SCREW THEORY BASED PATH TRACKING TECHNIQUES FOR AUTONOMOUS ROBOTS AND MANIPULATORS By WAHEED A. ABBASI A DISSERTATION PRESENTED TO THE GRADUATE SCHOOL OF THE UNIVERSITY OF FLORIDA IN PARTIAL FULFILLMENT OF THE REQUIREMENTS FOR THE DEGREE OF DOCTOR OF PHILOSOPHY UNIVERSITY OF FLORIDA 2001 Copyright 2001 by Waheed A. Abbasi ACKNOWLEDGMENTS I would like to express my thanks and appreciation to the members of my supervisory committee: Dr. Carl Crane, Dr. Joseph Duffy, Dr. John Ziegert, Dr. John Schueller, and Dr. Michael Nechyba for their support and guidance. I extend special thanks to committee chairman Dr. Crane, for his guidance, patronage, and support throughout my tenure. I thank Dr. Jose Rico Martinez for his invaluable input and his friendship. I also thank my friends and colleagues in CIMAR who provided a very cordial and friendly work enviroimient. I give my heartfelt gratitude to my parents, whose love, dedication, and encouragement made whatever I have done possible. Finally, I give special thanks to my wife, Uzma Abbasi, who provided continuous encouragement and the inspiration needed to finish this dissertation. " . - 'f^ ^ -^■^I». TABLE OF CONTENTS page ACKNOWLEDGMENTS iv ABSTRACT vii 1 INTRODUCTION 1 Motivation 1 Background 2 Research Objectives 2 2 BACKGROUND ON THEORY OF SCREWS 4 Definitions and Basic Concepts 4 Statics 10 Kinematics 15 3 REVIEW OF LITERATURE 20 Techniques to Improve/Modify the Path Planner 21 Techniques to Improve Path Tracking 29 4 PATH TRACKING METHODOLOGIES 45 Autonomous Robot / Vehicle 46 Considering Pose Information Only 50 Considering Pose and Velocity Information 51 3R Planar Manipulator 61 Considering Pose Information Only 61 Considering Pose and Velocity Information 67 Parallel Manipulators 76 Tracking Methodology 77 6-6 Parallel Manipulator 79 Algorithm Implementation 81 5 PATH TRACKING AND CONTROL 91 Manipulator Model 91 Motor Specifications 94 System Model 95 Controller Development 97 Controller Implementation 100 6 CONCLUSIONS AND FUTURE WORK 108 Conclusions 108 Future Work 109 APPENDIX A INITIAL PATH-TRACKING APPROACHES Ill B SIMULATION SOURCE CODE 116 LIST OF REFERENCES 155 BIOGRAPHICAL SKETCH 159 'r t ■ y •' ' ■ - VI Abstract of Dissertation Presented to the Graduate School of the University of Florida in Partial Fulfillment of the Requirements for the Degree of Doctor of Philosophy SCREW THEORY BASED PATH TRACKING TECHNIQUES FOR AUTONOMOUS ROBOTS AND MANIPULATORS By WAHEED A. ABBASI August 2001 Chairman: Dr. Carl D. Crane III Major Department: Mechanical Engineering The Theory of Screws, invented by Sir Robert Ball, has been used in the mechanisms area for over three decades. Engineers and scientists at the Center for Intelligent Machines & Robotics (CIMAR) have successfiilly used the Theory for a variety of applications including analysis of serial and parallel mechanisms, tensegrity structures, and vibration analysis. The theory provides for an elegant and geometrically meaningful way for conducting such analyses. Following a planned path is an essential element in most applications of robots and manipulators. Following a path necessitates the determination of how accurately the path is followed. This is generally known as path tracking. This research is aimed at applying the Theory of Screws to the path tracking problem. The research forays into developing a path tracking methodology for autonomous vehicles, serial robots, and parallel manipulators based on Screw Theory. vu The path tracking problem is addressed by developing mathematical basis for generating the tracking commands. The techniques are based on whether the tracking needs to be done at the position level or at the velocity level, i.e., whether the trajectory has to be continuous at the position level or at the velocity level. The basic models are developed including the resultant of combining a velocity and an acceleration command screw. The developed path tracking methodology is applied to autonomous robots and serial manipulators. A separate methodology is developed for parallel mechanisms. A technique is devised that satisfies the motion criteria for the desired position, velocity, and acceleration commands. The technique provides the connector displacements, velocities, accelerations, and forces for a prescribed motion of the platform. The methodology can be used as a mechanism performance evaluation tool. Simulations are performed to validate the results. A dynamic model of a 3R manipulator is developed to test the path tracking algorithm with a control system. A PID controller is used for manipulator motor control. The control system is modeled with a time delay, which is varied to study its effects on system performance. The results show that the path tracking techniques function for the test manipulator within a range of time delays. vm CHAPTER 1 INTRODUCTION Motivation The motivation for this work comes from the elegance with which the Theory of Screws has been used for the kinematic and dynamic analyses of robot manipulators. The initiative is to employ the Theory of Screws (Screw Theory) and attempt to develop a methodology for path tracking of robots. Following a planned path is a fundamental function in most applications of robots, manipulators, machine tools, and autonomous vehicles V In most cases, following a planned path is not only necessary but is critical in accomplishing the task; necessitating the determination of how accurately the path is followed. This is accomplished by what is known as path tracking. Path tracking in general can be defined as following a prescribed path with possible additional constraints. These additional constraints are generally system or task dependent. For robot manipulators and autonomous vehicles, these constraints can be orientation, time, velocity, and acceleration. A cursory review of scientific literature in the area of path tracking shows that path tracking is in general either a sub-system in the overall control scheme of the robot or is embedded in the control system. The term robot is used as a generality; it encompasses manipulators (serial and parallel), machine tools, and autonomous vehicles. Background Work in the area of mechanisms (among other research areas) has been ongoing at the Center for Intelligent Machines and Robotics (CIMAR) for decades. Research has been conducted and published on mechanism analysis, ranging from direct (forward) and inverse (reverse) analysis of mechanisms to velocity, acceleration, and jerk analysis of mechanisms. An area of particular thrust has been the design and development of parallel mechanisms for a variety of applications. Parallel platforms have been, and are being, investigated for applications ranging from passive feedback devices to milling machines. The development of mechanical robotic systems requires the need for a control system to accomplish the prescribed task that it is designed for. The idea of using Screw Theory to accomplish this task arose from this need. • Research Objectives The objective of this research is to investigate the use of Screw Theory as a tool for path tracking of robot manipulators. The intent is to develop a methodology that lays the framework for tracking control. Techniques will be developed for tracking control of planar serial manipulators (3R), parallel manipulators (6-6 spatial manipulator), and autonomous vehicles. These techniques are implemented in simulation and results presented to show the performance and viability of the techniques. The material is arranged in the following order. Chapter 2 offers a brief introduction to the Theory of Screws and to the concepts that are used in this research. Chapter 3 reviews the current state of literature as it pertains to the control of robotic manipulators in general, and to path tracking in particular. Chapter 4 presents the approaches that have been developed and the simulations that have been done to illustrate and validate the methodologies. Chapter 5 presents an application of the path tracking methodology to a 3R Manipulator with a system model and application of control strategies. Chapter 6 summarizes the pertinent work that has been done, concludes the results, and provides an outline for future work. *, .^ CHAPTER 2 BACKGROUND ON THEORY OF SCREWS Since the work presented in this manuscript derives from the Theory of Screws, a brief overview of the relevant topics in the theory are presented to facihtate understanding. This introduction is not intended to be comprehensive, just illustrative enough to familiarize the reader with the material that follows. After the introductory material, results pertaining to the velocity and acceleration analyses of mechanisms based on Duffy, Crane, and Rico's work are presented. Screw theory has been used in kinematics for more than two centuries. Sir Robert Ball conducted the principal and the most comprehensive fundamental work in this area at the end of the 19th century. This work was published as A Treatise on the Theory of Screws in 1900 [BalOO]. Except for a few isolated contributions in the first decades of this century, screw theory lay dormant until it was rediscovered in the 1960s by Hunt and Phillips [Hun78, Phi84] who made very important advances in the field. Since then, the theory has been used extensively in kinematics: fi-om the search of over-constrained linkages, grasping analysis, velocity and singularity analysis of serial manipulators, to force, velocity and acceleration analysis of parallel manipulators [Mar96]. Definitions and Basic Concepts Ball defined a screw in a manner that was impervious to whether its usage was in kinematics or dynamics. A screw is a geometrical identity described as "a straight line with which a definite linear magnitude termed the pitch is associated." In defining the pitch, Ball considers a rigid body that needs to go from one position to a desired position. A certain axis can be found such that if the body is rotated around that axis for a determinate angle and translated parallel to the axis for a determinate distance, the desired movement can be achieved. The situation is analogous to a body attached to the nut of a uniform screw (in the ordinary sense of the word) which has an appropriate position in space, and an appropriate number of threads to the inch. Thus the pitch of a screw is defined to be the rectilinear distance through which the nut is translated parallel to the axis of the screw, while the nut is rotated through the angular unit of circular measure. The pitch is thus a linear magnitude. It follows from this definition that the rectilinear distance parallel to the axis of the screw through which the nut moves when rotated through a given angle is simply the product of the pitch of the screw and the circular measure of the angle. Twist: A body is said to receive a twist about a screw when it is rotated uniformly about the screw, while it is translated uniformly parallel to the screw, through a distance equal to the product of the pitch and the circular measure of the angle of rotation. Whatever the movement of a rigid body, it is at every instant twisting about a screw. For the movement of the body when passing from one position to another adjacent position, is indistinguishable from the twist about an appropriately chosen screw by which the same displacement could be affected. The screw about which the body is twisting at any instant is termed the instantaneous screw . A system of forces acting on a rigid body may be generally expressed by a certain force and a couple whose plane is perpendicular to the force [Poi06]. A wrench is defined as a force and a couple in a plane perpendicular to the force. A wrench is thus a force directed along the screw and a couple in a plane perpendicular to the screw, the moment of the couple being equal to the product of the force and the pitch of the screw. It follows from this that if the pitch is zero, the wrench reduces to a pure force along the axis of the screw. The Lie product is a product of two screws It is also known as the Motor Product or Dual Vector Product and is defined as [$, $2] = [(<o,;Do,) (o>2;t)o2)] = (o), XCO2 ;(i),xDq2-(02XDq, ) C2.1) A more mathematical description of entities follows. Consider two distinct points ri(xi,yi zi) and r2(x2,y2,Z2) in space. The join of these two distinct points determines a line (Figure 2.1). The vector S directed along the line can be written as S = (r2-r,) (2.2) which may be expressed as S^Li+Mj + Nk (2.3) where L = X2-x^ ,M = y2-y^ ,N = 2.^-2^ are defined as the direction ratios. From Equation (2.3), the direction ratios are related to the distance \S\ by » '■:* Z'+M'+iV' = |S| . (2.4) / . Often the entities L, M, and N are expressed as |s| |s| |s| ^^-^^ which represent the unit direction ratios or the direction cosines of the line. In this case (2.4) reduces to L'+M^+N^ = 1. (2.6) Figure 2. 1 : Two points determine a line. In Figure 2. 1 , let r be a vector from the origin to any point on the line. The vector (r-ri) is parallel to S, thus ' ' ' , t •> t (r-r,)xS = 0, This equation can be written as where rxS = So So = r, X S (2.7) (2.8) (2.9) represents the moment of the line about the origin O and is origin dependent. Also, since the vectors S and So are perpendicular they satisfy the orthogonality condition 0. (2.10) A way of writing the coordinates of a line is in the form {S;So} and they are referred to as the Pliicker coordinates of a line. The semi-colon is used to signify that the dimensions of |S| and |So| are different. The coordinates {S;So} are homogeneous since from (2.8) the coordinates {A,S;A,So}, where X is a non-zero scalar, determine the same line. Expanding Equation (2.9) yields, • 1 J k x^ yx ^1 L M N (2.11) which can be written as where So = P\+Q\ + R\!i. P = y,N-z,M ., Q = z^L-x^N , (2.12) (2.13) From (2.3) and (2.12) the orthogonality condition S.So=0 can be expressed in the form LP + MQ + NR ^ (2.14) If homogeneous coordinates of two points in space are provided as (l;xi,yi,zi) and (l;x2,y2,Z2), the Pliicker coordinates of the line (Figure 2.2) can be expressed by using the Grassmann method by taking six 2x2 determinants of the array 1 X, y^ 1 1 j'\ ^1 1 X., y^ Zj (2.15) as L. = 1 X, 1 x^ , M = 1 >'■ 1 .^2 , iv = 1 ^, 1 z, P = y y I ^1 2 ^2 , e= Z, X, ^2 ^2 , R = X| X2 J ^2 (2.16) R M- L Figure 2.2: Plucker coordinates of a line. Equations (2.2) and (2.9) can be used to determine the Plucker coordinates of a line when two points on the line are given. It is also important to determine a point on a line when the PlUcker coordinates are given. If, for example, the coordinates of a line are given, {L,M,N;P,Q,R}, an infinite number of solutions exist for x,y,and z (corresponding to the infinite number of points on the line) [Cra99]. For the general case, any arbitrary value of X may be selected and the corresponding values for y and z may be determined as: y = xM-R z = xN + Q (2.17) L L The distance of a line from the origin is determined by the length of the vector p, which originates at O and terminates at a point on the line such that the direction p is perpendicular to the direction of the line, S. The distance of the line from the origin is determined as ■'^.-.^^■i 10 ISol |P| = 1^- , (2.18) When So=0, |p|=0 and the line passes through the origin and its coordinates are {S;0}. When S=0, |p|=Qo, the line is a line at infinity and its coordinates are {0;So}. A line can be defined by the join of two points as well as by the intersection of two planes. The array of coordinates {L,M,N;P,Q,R} is known as the ray coordinates for a line. The line can be considered as a ray of light joining two distinct points on the line. The array of coordinates {P,Q,R; L,M,N} is known as the axis coordinates for the line determined by the meet of two planes. In this case, the line can be considered as the axis of a pencil of planes. Pairs of dual elements, i.e., points or planes can thus form a line. It is because of this that a line in three-dimensional space is considered to be dual with itself or self-dual. A distinction is made in notation when the line coordinates are specified. The ray and axis coordinates are represented by s = {S;So} and S = {So;S} respectively. Statics Consider a force with magnitude f acting on a rigid body as shown in Figure 2.3. The force is acting on a line $ with ray coordinates s = {S;So} where |S|=1. The force f can be expressed as a scalar multiple fS of the unit vector S which is bound to the line $. In order to add forces it is necessary to introduce a reference point O. The moment of the force f about this reference point, i.e., mo can be expressed as mo=rxf where r is a vector to any point on the line $. This moment may also be expressed as a scalar multiple fSo where So is the moment vector of the line $, i.e., So^rxS. The action of the force on the body thus can be expressed as a scalar multiple of the unit line vector, and the coordinates for the force are given by 11 w = fs = f{S;So} (2.19) where S • S = 1 and S • Sg = . The above equation can be expressed in the form w = fs = {f;mo}. (2.20) Figure 2.3: Force acting on a rigid body. It can be seen from Figure 2.3 and mo=rxf that the vector mo changes as the location of the reference point is changed. When the line $ passes through the reference point, mo=^0 and the coordinates of the force are {f;0}. Clearly, f is a line bound vector which is invariant with a change of coordinate systems while mo is origin dependent. Consider a rigid body on which is acting a force {fi;moi} whose coordinates are expressed in terms of the reference point O. Equal and opposite coUinear forces whose coordinates are {fi;mo2} and -{fi;mo2} are applied to the body so that the net resultant force and couple acting on the body remain the same. The forces {fi;moi} and -{fi;mo2} 12 may be replaced by a couple which is equal to the sum of the moments of the two line- bound forces about point O, i.e. m=moi-mo2. The combination of the force {fi;mo2} and the couple {0;m} has the same effect on the rigid body as the original force {f;moi}. Now consider an arbitrary system of forces with coordinates {fi;moi}, {f2;mo2}, ..., {fn;mon} acting on a rigid body. It is assumed at the outset that a reference point O has been chosen, the magnitudes of the forces fi, f2, ..., fn are specified, and the unit coordinates of the lines of action of the forces {Si;Soi}, {S2;So2}, ...{Sn;Son} are known. The forces are translated to point O and moments moi, mo2, ... mon are introduced to yield an equivalent system of forces and torques that act on the rigid body. The net force acting on the rigid body is given by _ f = If.- = -4 - (2-21) and the line of action of this force passes through point O. The moment acting on the rigid body is given by m = Z™o-- " (2.22) The net force f (which passes through point O and whose coordinates are written as {f;0}) and moment mo (which is a non-line-bound vector whose coordinates are written as {0;mo}) are equivalent to the original set of forces. The coordinates of the resultant of the force and moment may be written as the sum of {f;0} and {0;mo} as w = {f;ino} (2.23) In general f and mo will not be perpendicular and the quantity with coordinates w={f;mo}, where fmo^O, was defined as a dyname by Plucker. It follows that in general it is not possible to translate the line of action of f through some point other than point O 13 and have the translated force produce the same net effect on the rigid body as the original dyname. The moment mo, however, may be resolved into two components ma and mt as mo = m^ + m, (2.24) which are parallel and perpendicular to the force f respectively. The moment ma may be determined as m, =(mo-S)S (2.25) where S is a unit vector in the direction of the resultant force f. The moment mt is then determined as m, = nin — m. (2.26) The line of action of force f may now be translated so that the force with coordinates {f;mi} plus the moment {0;ma} is equivalent to the dyname {f;0}+{0;mo}(Figure 2.4). The dyname can thus be represented uniquely by a force f acting on the line of action {S;Sot} where S,,, = — — and a parallel couple ma. This representation is called a wrench and is attributed to Ball. f ■ \j . ^, '/;•■ rxf=m. y Figure 2.4: A Dyname and a Wrench. 14 The wrench w which is equivalent to the dyname {f;mo} may be written as w = {f;mo}= {f;m,}+{0;inj (2.27) It is desirable to express the wrench in terms of f and mo. Since ma is parallel to f m^ = hf (2.28) where h is a nonzero scalar which is called the pitch of the wrench. By algebraic manipulation and using Equation (2.24), the pitch of the wrench is given by h = -p-p (2.29) Clearly the pitch h is an invariant quantity. From (2.26) and (2.27) we have w = {f;mo-mJ+{0;mJ (2.30) and substituting ma from (2.28) gives w = {f;mo-hf}+{0;mj. (2.31) The homogeneous coordinates for the line of action of the wrench are {f;mo-hf} and the equation for the line is therefore rxf = {f;mo-hf}. (2.32) In the same way as the action of a force upon a body can be expressed as a scalar multiple of a unit line vector, a wrench acting on a body can be elegantly expressed as a scalar multiple of a unit screw where s = {S;So} (2.33) and S ■ S = 1 . The pitch of the screw is given by h = S-So. - (2.34) From (2.33) s = {S;So} = {S;So-hS} + {0;hS} line couple The Pliicker coordinates of the screw axis are {S;So -hS} and the equation of the axis is 15 rxS = So-hS. (2.36) A screw can therefore be regarded as a line with a pitch. Kinematics In this section a brief overview of the resuhs relevant to the approaches in this research are presented. References are provided appropriately for the reader interested in details about the derivation and proofs of the results. The results presented here pertain to the velocity and acceleration analyses of serial manipulators (open chains). The detailed description of these analyses is presented by Rico and Duffy [Mar96, Ric99]. The velocity analysis of a serial chain consists of two parts. The first part relates the angular velocity of two arbitrary links in the serial chain, while the second part relates the velocity of a point fixed in one link as observed from different links (or reference frames) of the serial chain. The angular velocity of a rigid body m with respect to a body (or reference frame) j, can be expressed, in terms of auxiliary bodies (or reference frames), m-\, m-2, . . . j'+2 aadj+l as follows: ^co" = ^(o^'*' + ^'^'o)^"^ + ... + "-^(d"-' + "-'to'" . (2.37) where ""'a)" represents the magnitude and the direction of the angular velocity of body n as measured with respect to body n-1. It should be noted that in this proposition, it is not necessary that the bodies be physically connected at all. Now consider a point O fixed in body m of a serial chain with respect to the reference frame j. The velocity of point O in body m as measured with respect to reference frame _/ is given by 16 '< y+io;+2 m-2c "J"^ m-I c^m j(o.^, ^sr + ;.,cy,,2 ^"sr + ... + „-26>._, "-^So-- + „_,«„ -'S: (2.38) where ^_^(o„ represents the scalar magnitude of the angular velocity of body n as measured with respect to body n-1. The velocity state of a rigid body m with respect to a reference frame y, is defined as ^ym _ ^(1)'" 'y>: (2.39) The velocity state parameters ^co"" and ^Vq are often referred to as the screw coordinates, $, and are written as ^S"- = { ^co'"; X}- (2.40) As an example, consider a two-link manipulator. The velocity state of link 2 with respect to the base link can be expressed as 0*2 _ Offl , la.2 ^$' + '$% (2.41) where 0<p2 $^ = "$' = '$^ pco^l ["s'l >o. = 0fi^2 ho'l ["s'l = ^CO^ 0<jl ^0. ['(O^' ps^l >o. = ,«2 1^2 ' (2.42) (2.43) (2.44) and 'S-' are unit vectors. In XhQ forward velocity analysis, the angular velocity for each revolute and helical joint and the linear velocity for each prismatic joint are known. The objective is to 17 determine the velocity state of the last link of the manipulator relative to a reference frame (base). The velocity state can simply be written as (e.g., 6-link manipulator) «(0^ = 0^1 °s' Oci + ,«2 Ic2 lc2 + ^(O, 2^3 + 3^4 3c4 3c4 + a(Os 4c5 4c5 + 5^6 556 (2.45) This may be written as l<r2 2<p3 3a.4 4ff5 50-6 = o^y, $' + ,6>2'$' + ^0)/$' + jCo/S^ + ^0)^^$' + 5(y/$ (2.46) where '$^ are the appropriate unitized screw coordinates for each joint axis. This maybe rewritten in the matrix format •^0 = nto/S" = J0> o^'e (2.47) where J, called the Jacobian matrix is the 6x6 matrix formed from the unitized screw coordinates as J = [<•$' '$^ ^$^ '$" "$' '$^] (2.48) and (0 is the vector of joint velocities given as 0) = [o«, ,<y2 ^co^ ^(o, ,(o, ^(o^ . (2.49) From the position analysis all the elements of J are known, coupled with the angular velocities being known, (2.47) can be solved to determine the velocity state of the end effector. For the reverse velocity analysis, the Jacobian matrix is again known coupled with the velocity state of the end-effector relative to the base. The individual joint velocities can be obtained from 18 CO = J "(0^ 0.,6 Dr, (2.50) If the Jacobian matrix is singular, and thus its inverse cannot be obtained, the manipulator is referred to as being in a singular configuration. For the acceleration analysis, open serial chain is considered, where body m is an arbitrary link and the rigid bodyy is the reference frame. Further, it is assumed that adjacent bodies or links of the serial chain are connected by helical pairs, and O is an arbitrary point fixed in the reference frame. The reduced acceleration state of an arbitrary link m, with respect to the reference frame y, is defined by ^a" /a^-^<x^< (2.51) and in terms of screws connecting adjacent links, is given as ^A: = ..»..., ^%J'' + ..,,<y.,., J''%^^^ + ... + .„ ,0, . "-^S""-' + ^_j«^ '"-'$'« + m-2^m-l + ... , . . .- (2.52) where [' %^* $ " ] is the Lie product of the screws inside the bracket. This can also be written as ^X1 = [J]ci, + [L], (2.53) where J is the Jacobian and L is the summation of all the Lie products. The Lie product of two screws is also known as the Dual Vector Product and is defined in the following manner. Consider two screws $i and $2, 19 ($, $2) = fe;i^oi) ('y2;t5o2)) (2.54) then the Lie product is [$, $2] = fex^2) ; fe X '^02-^2X^^01 )]• (2.55) Rico and Duffy have further extended this analysis to the jerk [Rjc99]. CHAPTER 3 REVIEW OF LITERATURE Conventionally, path tracking falls under the umbrella of robot control algorithms, which are typically divided into two stages: path or trajectory planning and path tracking (path control). Path tracking is generally considered the lower level of control and path or trajectory planning is considered the higher level. The path tracker attempts to make the robot's actual position and velocity match some desired values of position and velocity, which are provided to the controller by the path planner. Although the terms path and trajectory are used interchangeably, a distinction is sometimes made: the path is a time-independent description of the route tracked by the manipulator, and the trajectory is a time-dependent description of the route. A variety of approaches have been undertaken to improve the path following performance of robots. These approaches vary from modifying the 'path planner' to changing the 'path tracker' to an amalgam of both. In some cases, the distinction between the two is transparent or is lost. Other approaches attempt to optimize a particular criterion in robot control, for example, tracking time or joint torque. The approach taken in this research is different because it uses Screw Theory for path tracking. Literature on this research is lacking. Nonetheless, I present an overview of the techniques that are most relevant in the following sections. I attempt to group literature under broad categories although the reader is warned that each approach is relatively distinct. ;»^ I f i 1* ' 20 21 Techniques to Improve/Modify the Path Planner Robot trajectory refers to a time history of position, velocity, and acceleration for each degree of freedom. Many robot applications require the end-effector to track a specified path for the purpose of accomplishing the task satisfactorily and avoiding obstacles. For achieving this objective, the trajectory planning can be conducted either in the joint- variable space or in the Cartesian space. The earliest work of Shin and McKay [Shi85] attempts to improve the trajectory planning stage of robotic manipulator control. The authors claim that robot control is divided into path plarmer and path tracker components because the process of robot control, if considered in its entirety, is complicated. Dividing the controller into two parts makes the process simpler. The path tracker is generally a linear controller and the non-linearities of manipulator dynamics frequently are not taken into account at this level. The simplicity obtained from this division comes at the expense of efficiency and the source of this inefficiency is the trajectory planner. The authors assert that to be able to use the robot at maximum efficiency the trajectory planner must be aware of the robot's dynamic properties. To partially alleviate the inefficiency of the planner the authors present a solution to the minimum-time manipulator control problem' subject to constraints on its geometric path and input torques/forces. The output of the planner is the true minimum-time solution. ' " • '"-" ' ' The problem is set up by defining a dynamic model for the manipulator using the Lagrangian formulation. The motion is constrained to a fixed path in joint space and the path is given as a parameterized curve f(k), where X varies from to X,max. The derivative of A, with respect to time is taken as ju. The resulting equation is in terms of X and //. If 22 the parameter X is considered as arc length in Cartesian space, then // is the speed and /i the acceleration along the geometric path. After setting up the dynamic equations, the control problem is addressed. From optimization, the minimum cost is equated with minimum time, which results in optimizing the operating speed of the robot. This is called Minimum Time Path Planning (MTPP).The cost function is optimized with the constraints on the velocities, slope of trajectories, and joint torques, as equalities or inequalities, thus yielding regions of admissibility and islands of inadmissibility. These regions provide the acceptable bounds for trajectories. Then an algorithm called the Algorithm for Constructing Optimal Trajectories (ACOT) is used to determine the optimal trajectories. The optimal trajectories are determined by using the algorithm with the bounds determined previously using MTPP. The optimal trajectory may actually touch the boundary of the admissible region, generating a dangerous case. It is pointed out however, that if slightly conservative torque bounds are used in the calculations, then the actual admissible region will be slightly larger than the calculated admissible region, giving some margin of error. Previously, Luh et al. [Luh77, LuhSl] had proposed the use of linear and nonlinear programming in trajectory planners to generate desired positions, velocities, and accelerations. These methods assumed that the desired path was given in terms of the path's endpoints and set of intermediate points or comer points. Along each segment of the path the (constant) maximum accelerations and velocities were given. In general, these constant bounds may have been quite inaccurate for some parts of the segment, since worst-case bounds for the whole segment had to be used. Shin et al.'s [Shi85] The problem of moving a manipulator in minimum time along a specified geometric path. 23 method was an improvement over these techniques since it calculated the acceleration and velocity limits directly from the path, the manipulators dynamic equations, and actuator characteristics. Another approach was taken by Shin and McKay [Shi86a] in an attempt to address some issues that were not fully addressed by the MTPP method [Shi85]. The authors point out that in the MTPP approach, the solution works only for minimum-time problems. In situations where driving the robot consumes large amounts of power, the assumption that minimum time is equivalent to minimum cost may not be valid. Another assumption is that joint torques can be changed instantaneously. Also, sometimes the actuator torque limits are dependent on one another as in the case when a robot uses a common power supply for the servoamplifiers for all joints. To rectify the shortcomings of the previous approach, the idea of using Dynamic Programming was proposed to find the optimal phase plane trajectory {X vs. /J). The advantage of using Dynamic Programming over previous methods that used linear or nonlinear programming [Luh77, LuhSl] is that it places few restrictions on the cost function that is to be minimized. Putting limits on jerk is possible, and the interdependence of torque bounds can be handled easily. The authors call this approach the minimum-cost trajectory plarming (MCTP). The obstacle avoidance problem is bypassed as it is assumed that the path has been specified a priori and the problem becomes: what controls will drive the robot along a specified curve in joint space with minimum cost, given constraints on velocities, control signals and their derivatives? As done in their previous work, the authors reduce the complexity of the control problem by introducing a single parameter that describes the ... .- ,' ■■■■ *. ; -. - ^ ' ■ _ ^ ■ robot's position in the form of a parameterized curve in the robots joint space. This single parameter and its time derivative completely describe the current state of the robot. The problem then becomes a two-dimensional minimum-cost control problem with some state and input constraints. To apply dynamic programming to this problem, in which the dimensionality has been reduced to two state variables, the phase plane is divided into a grid. The cost of going from one point in the grid to next point is calculated. Since all the dynamic equations are strictly given in terms of the state variables, the cost computation is done entirely in phase coordinates. Once costs have been computed, the dynamic programming algorithm is applied to determine positions, velocities, and torques, from the resulting optimal trajectory, and the curve and dynamic equations. The significance of the results is that they provide a simple approximation to the truly optimal trajectory problem to any desired degree of accuracy. It must be noted however that the effect of grid size on the quality of results is of practical importance. The grid should be fine enough to provide good results but not too fine to cause excessive computations. The varying of column size and row size in the grid will yield different effects on the results. Varying the number of columns (k divisions) varies the accuracy of the dynamic model while varying the number of rows (ju divisions) varies the accuracy of the approximation to the true minimum-cost solution. Another important factor is the ratio of the number of rows to the number of columns so that the slope of the curve connecting two adjacent points in the grid is small. ^ The task planner generates a sequence of Cartesian points and additional intermediate knot points which, if necessary, are transformed pointwise to points in joint space. These joint points are interpolated to form a geometric path in joint space [Pau81]. 25 Another paper authored by Shin and McKay [Shi86b] addresses the issue of finding the geometric path. The problem states that given the solution to the minimum time trajectory planning problem, find the geometric path or the trajectory curve so as to minimize the traversal time. Xiangrong and Xiangfeng [Xia94] present a method for trajectory planning that includes Cubic Polynomial (CP) motion trajectory plaiming and addresses the minimum- time trajectory problem. Their work extends and improves the work done on a similar problem by Luh et al. [Lin83]. It uses Cartesian space schemes for trajectory planning. The characteristics of CP motion are that the end-effector not only must achieve the final target point, but also must move along the desired path. The time history of the positions, velocities, and accelerations of the end-effector are assumed to be prescribed, and the corresponding joint displacements, velocities, and accelerations are obtained from a given position and orientation of the end-effector by solving the inverse kinematics. If T is the time needed for a robot to perform a CP motion, the time interval 0-T is divided into smaller sub-intervals. At each interval computations are done for the transformation matrix for the end-effector coordinate with respect to the base coordinate using the equations of the path and constraint conditions for the end-effector position and orientation. Inverse kinematics is used to determine the velocities and accelerations of each joint. After this information is obtained for two successive intervals, a 3^'' to 5* order polynomial is used to specify each joint trajectory in that sub-interval. If there are no constraints on velocities of each joint except at initial and final points, a cubic spline fit can be used for all intermediate points and a 4"" order for the first and last point. The 26 coefficients of polynomials at each segment can be obtained recursively and hence the trajectories for all sub-intervals are planned. The next step is the Minimum-Time Trajectory planning. It is assumed that the maximum velocity, acceleration, jerk, and torque for all joints are specified and are used as constraints. A new scaling factor is determined which attempts to maximize (»1) the ratio of current value/maximum value (of velocity, acceleration, etc) during a sub interval. A new trajectory is determined recursively using the former method until the maximum ratio is obtained at which point the minimum-time joint trajectories are considered obtained. The paper discusses an additional issue of determining the number of time sub- intervals. A maximum allowable deviation of trajectory is assumed. Using the CP Trajectory algorithm the trajectory is planned. From this information the corresponding actual position of the end-effector in Cartesian space is determined. This is compared with the trajectory, and if it is not within the allowable deviation, the sub-interval is divided into two parts by adding an intermediate point and the same process is repeated until an acceptable time interval is obtained. Piazzi and Visioli [Pia97] have presented an approach to find the minimum-jerk cubic spline joint trajectory of a robot manipulator using interval analysis. Minimum- jerk trajectories are desirable for their amenability to path tracking and to limit robot vibrations while cubic splines are used to ensure continuity of velocities and accelerations in the robot movement. The importance of minimizing the jerk in trajectory planning is primarily due to the fact that joint position errors increase when the jerk increases. Moreover, minimum- 27 jerk joint trajectories are desirable for their similarity to human joint movements and to limit robot vibrations. For achieving this goal, a global optimization method, based on interval analysis, is presented. The path of the manipulator is specified in Cartesian coordinates and is converted to joint space via inverse kinematics. The sequences of joint displacements are interpolated by use of cubic polynomials to ensure an overall continuity of position, velocity, and acceleration. Using the total elapsed time for the displacement, a parameterized spline is obtained that incorporates continuity of positions and velocities. The optimal trajectory planning problem with minimum-jerk criterion is then posed as a constrained minimax optimization problem. The Interval Algorithm [Moo79] is used for the global optimization problem and is called the Minimum-Jerk Interval (MJI) algorithm. An example is presented for a 2D0F robot to illustrate the minimum-jerk joint-space trajectory planning with continuity of velocities and accelerations using the interval algorithm. Vv I :• According to the robot's dynamic model that is employed, trajectory planning can be done either continuously or discretely. Techniques such as of [Shi85] employ a phase- plane technique to obtain the optimal solution for the trajectory planning problem. In these algorithms, constraints are considered to make full use of the capacities of the joint actuators and a true minimum time solution is yielded. It should be pointed out that these algorithms work for a small range of trajectory planning problems [Du95] that consider only generalized force constraints. Other realistic constraints such as joint velocity limits, jerk constraints, and end-effector velocity limit are neglected. 28 A discrete model is, however, more appropriate than a continuous model in solving the trajectory planning problem [Fla88]. In this method the whole generated path is discretized by individual points. The obtained trajectory of the discrete points that optimizes the given performance index is thus an optimal one. The trajectory planner presented by Flash [Fla88] is based on a linear programming model. The use of an objective akin to the minimum time objective rather than the true minimum time objective function makes it possible for linear programming to be used, which allows fast computation. However, in this method, robot dynamics are not considered. Tan and Potts [Tan88] extend this method by considering non-linear constraints, such as generalized forces and joint jerks, and use nonlinear programming to obtain the optimal solution. In their method, the discrete trajectory plaimer is modified by taking all time intervals to be the same, which is difficult to execute as it is computationally extensive. A new method for segmental discrete trajectory planning, in which the whole trajectory is planned segment by segment along the path is presented by Du et al. [Du95]. The consistency of the time interval at the trajectory planning stage with the sampling time interval determined at the trajectory tracking control stage is considered. The method reduces the computation memory and run time. Path planning is often times categorized as either being static or dynamic, depending on the mode of available information. Static path planning is used when all the information about obstacles is known a priori. Most path plarming methods are static. Dynamic path planning is used when only partial a priori information is available about the obstacles, and the environment is unpredictable and time-varying. Zelek [Zel95] has presented an approach for dynamic path planning. Dynamic path planning is 29 problematic because the path needs to be continually recomputed as new information becomes available. Dynamic path planning is accomplished by using a harmonic function as the potential function to model the free space. Computation of the harmonic function is formulated as an iterative process and is performed independently of the execution of trajectory commands. In order to achieve continuous robot navigation with guaranteed control, there are coarse-to-fine set of harmonic computations being continuously carried out in parallel. Techniques to Improve Path Tracking As alluded to before, since robot dynamics are highly nonlinear, coupled, and of high dimension, time-optimal control is difficult: to simplify the problem it is commonly divided into offline trajectory planning and online tracking. The papers reviewed above have addressed only the trajectory planning problem, and have done so considering only the robot dynamics and not the tracking controller dynamics. These solutions have used the path constraint to reduce the dimension of the problem to 2, the path position and velocity, and solve the problem using a variety of optimizing techniques. These solutions provide mathematically correct time-optimal trajectories for any given robot model. However, their application to real systems is not so straightforward. Because the robot dynamics are never precisely known, the resulting trajectories are too approximate to be applied open-loop. The path constraint, which is so useful in reducing the dimension of the optimal control problem, preempts consideration of tracking error. The resulting time-optimal solutions push actuators to their limits, leaving no margin for controlling tracking errors. 30 Some published results have addressed these issues, including an approach by Shin et al. [Shi87] that proposes planning trajectories that are robust to payload uncertainties. Cahill, Kieffer, and James [Cah96] proposed a scheme for planning robust "time-optimal" trajectories for robots under computed-torque control. This scheme was based on the experimental identification of un-modeled dynamics as joint acceleration disturbances and a theory for relating those disturbances to torque margins that should be held on reserve during trajectory planning. The scheme also involved the selection of controller gains to ensure that tracking is accurate to a prescribed tolerance. Kieffer et al. [Kie96] in continuation of their previous work [Cah96] extend the approach to a more general architecture for online trajectory generation. Their work proposes a state feedback law that ensures nearly time-optimal path tracking which is accurate to a prescribed tolerance in the presence of experimentally identified levels of disturbance. The input path acceleration (s) is generated online as a feedback function of the plant and trajectory generator states and is thus called a closed-loop trajectory generator. The model for the robot is developed which is inclusive of joint acceleration disturbances. These disturbances can be identified from online measurements. The tracking error is then predicted and compensation torques are computed based on the error. A methodology is developed for choosing controller gains to meet a prescribed tolerance for tracking accuracy and for predicting levels of compensation torque that will develop online. With the theory developed to this point, the authors had previously employed dynamic programming to robustly determine time-optimal trajectories [Cah96]. 31 In the current instance, they extended it to Robust Closed-Loop Trajectory Generation. The idea is to determine how much the path acceleration {'s) can be modified without saturating any actuators which is necessitated by the fact that the equations predicting the tracking error and compensation torque are only valid if the torque commands are implemented without clipping. The closed loop architecture was implemented on the first two links of a SCARA robot arm. The robot model, identified by least squares techniques, included all rigid body inertial effects as well as independent coulomb and viscous friction terms for each joint. The task was chosen to be the same as the one used when the previous technique was implemented for comparison purposes. The results were successful, as the path velocity of the closed-loop scheme was everywhere greater than or equal to that of the previous scheme. The total elapsed time for traversing the trajectory was also improved and was very close to the model's time-optimal elapsed time. Torque utilization also increased substantially and exact saturation occurred most of the time. Also, the closed- loop system tracked the path within the specified tolerance, and that the errors were very similar to those of the previous robust trajectory planning scheme. It was concluded that the tracking times were reduced while the tracking accuracy and robustness remained approximately the same. Keiffer et al. have presented both the techniques with extensive experimental results in [Kie97]. ; .. ? * ^ v • ^ , Another approach to address the tracking problem involves developing methods for path planning and path tracking simultaneously. This approach is taken by Tsuchiya and Egami [Tsu90], and they assert that the best whole system is synthesized by combining them suitably. They have proposed robot path control with variable speed 32 trajectory planning and an assortment of control methods for tracking. They present a new trajectory plarming method that takes the control ability of the designed manipulator control system into consideration. This is done systematically without trial and error on the basis of the frequency response of the designed control system. This trajectory planning method is implemented with the following control methods: Adaptive control, Preview control, and Zero-error tracking control. An important issue at the trajectory plaiming stage is how to utilize the freedom of tracking speed. The purpose of the trajectory plarming method in this paper is how to carry out trajectory planning so that the system can utilize the maximum ability of the designed control system. The results show that the tracking speed of the robot manipulator in both the work space and joint space changes with the curvature of the given path. This trajectory planning method is known as "variable speed trajectory plarming." The method proposed by the authors differs than the traditional variable speed trajectory plarming in the sense that the ability of the designed control system is explicitly taken into account. ~ *^ The methodology for the planning entails a series of procedures and starts by the synthesis of the manipulator control system. By using the Bode plot of the control system the maximum angular frequency that guarantees no gain deterioration and phase delay is determined. The minimum angular frequency (coo) amongst each joint servo system is determined. This is referred to as the standard angular frequency and the robot can track the path with an angular frequency lower than this. The given path in Cartesian space is then converted into joint coordinates. A reference point sequence is obtained by plotting equidistant points on the transferred path by assuming that the robot manipulator 33 tracks the path with a constant speed. A circle is obtained consisting of three consecutive reference points on part of the tentative desired trajectory. From the circle, the angular frequency of the sinusoidal signal (odi) to be applied to the servo system to draw the circle is obtained. The points are taken as reasonable reference points on the path if coi= (Oo, otherwise the points are moved in appropriate direction until the condition is satisfied. A circle consisting of three consecutive points approximates each part of the given path. By using this method a desired signal is obtained as a function of time that can be applied to each servomotor; this signal is always part of a sinusoidal function with fixed angular frequency. Utilization of future information is useful for improvement of system response if available, and usually, the path tracked by a robot is available in advance. In this context. Preview Control utilizes future information and has many attractive characteristics over usual control systems. Although even simple application of preview action is effective, application along with variable speed trajectory planning is even more effective in reducing tracking error. In order to determine the desired signal for each servo system of each arm, trajectory planning is carried out based on the frequency response of preview control. A characteristic of this control system is the existence of feed-forward compensation utilizing a future desired signal. The transfer function of the preview control system from desired signal to controlled variable is obtained and the maximum angular frequency from the Bode plot is determined. The servo system can respond to the desired signal with angular frequency coo without any gain deterioration or phase delay. 34 Another control method employed with variable speed trajectory planning is the Zero-error Tracking controller. Zero phase error tracking controller is proposed to cancel the effect of unstable zeroes of a stabilized closed-loop system. A unity gain controller can be derived on the basis of zero phase error controller when the angular frequency of the desired signal for the control system is known and fixed. In general, this type of controller is not useful because such a situation rarely exists in usual engineering problems. However, the condition of constant angular frequency of the desired signal is always satisfied by variable speed trajectory planning. Nonlinear compensation methods are further employed to eliminate the nonlinear effects. The authors propose a third method for controlling the robot that employs the variable speed trajectory planning method. Adaptive Control is used as a countermeasure to parameter variations of the robot manipulator during an operation. Specifically, model reference adaptive control (MRACS) is applied to robot path control. By using the MRACS the control system is decoupled. This decoupling is very convenient for application of variable speed trajectory planning method as the method is essentially for single-input single-output systems. It requires plotting of Bode plot for each servo system. Simulation results for the third method are presented. It is noted that the results for the other two control techniques were omitted, as the results were more or less the same. Adaptive control (MRACS) is used with constant speed and variable speed trajectory planning. It is noted however that robot path control is considerably improved by applying the proposed trajectory planning with the significance of the said method being its non-reliance on trial and error methods. The input torque restriction of the 35 servomotors is not dealt with and reference is made to the work of Shin et al. [Shi85] that addresses this issue. Abe and Tsuchiya [Abe92] pubUshed work that refined and detailed the work published earlier on variable-speed trajectory planning by Tsuchiya and Egami [Tsu90]. Wang, Tsuchiya, and Hshimoto [Wan93] presented two control methods for path tracking: a digital control method based on the principle of movement of a nonlinear mechanical system, and a preview control method utilizing future information of the desired trajectory to improve the tracking performance of the former. A digital control law was derived and disturbance terms (functions of position and velocity) were included that otherwise may have been unknown. Wang and Tsuchiya [Wan94] proposed a new trajectory planning method based on high-level qualitative judgement. Fuzzy reasoning was used as the tool for this. The method is called fuzzy trajectory planning. This effort was to address unresolved problems in the previously proposed variable-speed trajectory planning method. The proposed trajectory method takes into account the important efficiencies and limits of a robot manipulator. Fuzzy reasoning [Zad69] is considered a powerful tool for making brief judgements and with the algorithm being very simple, it is used for the trajectory planning problem at hand. The requirements for setting the trajectory planning are set as: • The attained trajectory must be efficient for calculation and for putting into practice; • Its position, velocity, and (except at the starting point) acceleration must be smooth functions of time; and • Trajectory planning must be done without any trial and errors, keeping in consideration the limits of the actuator torques and the characteristics of the control system. 36 The variable speed trajectory planning suffered a few shortcomings in regards to the aforementioned requirements. The method required different processes for starting and stopping, i.e., the planning method had to be changed for the starting and ending part of the path. The method did not handle the torque limits on the actuators positively, and the algorithm was too complex. The fuzzy method proposed attempts to solve these issues at a certain level. In fuzzy trajectory planning, the rough knowledge of the robot manipulator and the control system is first expressed at the language level. Then by using a membership function of the fuzzy set, these expressions are transformed into fixed quantities. And then trajectory planning is done using the fuzzy reasoning method. Therefore, the most important step in fuzzy trajectory planning is how to acquire the rough but important knowledge about the robot manipulator and the control system, and how to make them abstract. Abe and Tsuchiya [Abe92] have already made use of the frequency characteristics of control systems and have explicitly taken the efficiencies of the control systems into account. The same information is adopted for fuzzy reasoning and for setting up a rule base. Next, the issue of velocity and torque limits is addressed. Once again, the variable speed trajectory planning provides the solution so that the velocity limits are not exceeded. The velocity of a robot manipulator moving along a path in a joint angle space can be determined by three items: the radius of curvature, the angular frequency of the trajectory, and the distance L from a sharp bend. All these are treated as fuzzy variables and constitute the fuzzy reasoning rules. 37 Once all the information is available at the language level, it is expressed in qualitative form by using membership functions of fuzzy sets. The rules for reasoning are set up. By using the rules, the reasoning results are obtained followed by deflizzification to get the final solution for the velocity to move along the path. While the issue of limiting torque is not addressed, it is pointed out that the information obtained from the reasoning results can be used to calculate the required torque. If this torque is within the prescribed limits then no action is needed. If not, then a search for a suitable new point near this point is done by moving along the path, taking very small steps. From the new point, the trajectory planning is carried out again by using flizzy reasoning. The authors concede that the problem of torque limits is a difficult problem to handle systematically at the trajectory planning stage and that although their method does consider torque limits, there may be cases where it exceeds the torque limit. The way to correct is to check for the exact value of torque, and if it exceeds the limit, reflect it immediately in trajectory planning. This will increase the processing time but the time of calculation for the fuzzy method is shorter than the variable-speed trajectory method. In summary, this method can obtain the trajectory without trial and error by considering the efficiencies of the control system and motor limits. It has a fairly simple algorithm, which, in some cases, may allow trajectory planning in real time. Also, the position, velocity, and acceleration form a smooth fianction in time. Grasa, Volberg, and Polyakhov [Gra93] present an approach that combines direct adaptive control and fuzzy logic control. To determine the coefficients and terms of the robot dynamic model and unmodeled dynamic effects, the authors use a combination of the computed torque method and the velocity gradient technique. This provides a simple 38 way to obtain some known and unknown parameter adaptive control laws, in particular, the exponential path tracking adaptive control algorithms. The algorithms require online measurements of the tracking error and its first derivative. The fuzzy logic control strategy is employed to attain implementable algorithms based on the developed adaptive laws. Many model-based controllers neglect non-linearities or system dynamics to simplify the design [Mas99]. These omissions may lead to poor performance or instability. This is especially true for Multi-Input Multi-Output (MIMO) systems. Mason, Walchko, and Crane [Mas99] have presented the development and implementation of MIMO hybrid fuzzy controller for a two-link robot manipulator to deal with the problem of designing MIMO control laws for systems with significant nonlinearities in the dynamics. The controller is aimed at providing better tracking performance of the robot manipulator. The method utilizes the designer's intuition (heuristic knowledge) and the mathematical description of the system. The controller combines the traditional proportional-derivative (PD) control law with a fuzzy controller in a MIMO framework. This is done because hybrid controllers provide a more defined control structure over fuzzy control while they improve the accuracy and portability of the controller. The MIMO fuzzy controller is formulated by extending the SISO (single-input single-output) fuzzy concepts to the vector domain. The control vector is resolved into a direction, magnitude, and position-derivative ratio and by doing so the MIMO problem for solving for a control effort is reduced to a scalar one. The system is further simplified 39 by making assumptions about the system (e.g., uncoupled), and the complexity of the fuzzy inference system is reduced. i For the purposes of benchmarking, a PD and a Feedback Linearization (FL) controller are also developed. Simulations are conducted to examine the performance of a fuzzy MIMO hybrid controller in ideal conditions. These results are compared against the PD and the FL schemes. Another set of simulations is done to study the effects of an unknown load disturbance. In the first case, the FL and the PD schemes produced a larger steady state error than the fuzzy scheme and their control effort was higher too. In the latter case, with random and deterministic disturbances, the fuzzy controller was better able to account for these disturbances. The control effort in this case was the same for all controllers. It was concluded that the MIMO fuzzy controller outperformed the other controllers. This scheme is more robust to random and deterministic disturbances and also offers portability to other robots with minimal effort. Najson and Kreindler [Naj96] have proposed a discontinuous robust control law for rigid manipulators to track a desired trajectory. The proposed control laws exploit manipulator properties and do not require the measurement of accelerations and the inversion of the inertia matrix. They require the knowledge of the present value of the desired trajectory and its first and second derivatives, rather than an entire predetermined trajectory for a given task. This enables on-line task changes and model following can be accommodated. The authors set out by deriving the dynamic equations of a rigid manipulator. The equations are transformed to a form suitable for adaptive control as established by Slotine and Li [Slo87]. A robust tracking control law is derived for the discontinuous 40 case, which is a state-feedback control law. The global, exponential stability to zero tracking error of the law is proved by a Lyapunov method and has exponential stability. A continuous approximation of the control law is then developed. This is done to avoid chatter in the system; while this causes the error to not exponentially tend to zero, it causes it to ultimately converge to an arbitrarily small neighborhood. This is what is referred to as practical stability. Once the controller is designed, adaptive ideas are incorporated. The gains are determined adaptively instead of being fixed in advance. An example is presented with simulation results for a two-link manipulator with parameter and disturbance uncertainties, including motor dynamics and friction. The continuous control law showed excellent robust tracking. The tracking error was reduced to near zero in about 0.8 seconds. It is noted that the control law does not take into account the uncertain motor dynamics added in the simulation. Dakev, Chipperfield, and Fleming [Dak96] have presented an approach to determine approximate solutions of path following optimal control problems by exploiting modem global search and optimization techniques. According to the methodology developed, controls are represented by discrete vectors and substituted in the system equations. Alternatively, when it is possible to exclude the controls from the system equations, a discrete vector represents the parameterization function of the path. In either case, the components of these vectors are regarded as variables of a performance index based goal ftinction that is to be minimized with respect to the control and phase constraints. Employing such schemes allows the modeling and solution of both open-loop and closed-loop path following optimal control problems within a unified framework of 41 function approximation and constrained optimization techniques. It allows for implementation of genetic algorithms for global optimization, multiobjective control, and utihzation of parallel processing. An optimal path following algorithm for control of multi-body systems is developed, and is tested for minimum-time-energy control of two-link manipulator. The path following optimal control problem is reduced to a constrained optimization problem by introducing a parameter set in order to approximate the control inputs. In the open- loop case, controls are regarded as the function of the time variable, t, while in the closed-loop case they are considered as functions of the state variables. Consequently, the original open-loop optimal control problem is reduced to a nonlinear programming case and the closed-loop problem is reduced a standard optimization problem. Once the problem is set up, the goal function and associated constraints are evaluated and the global optimum of the constrained optimization problem is found. In the case of closed- loop control, identification of the control law based on the input/output relations obtained from the solution of the open-loop optimal control problem needs to be done. The methodology was tested by simulating a model of a robot with one revolute and one prismatic joint with both open-loop and closed-loop optimal control. Li, Tso, and Zhang [Li98] have proposed a dual-model-based structure for uncertainty attenuation in the trajectory-tracking control of robot manipulators. The traditional advanced control schemes are model based in the sense that a dynamic model, described either explicitly as a mathematical equation or implicitly as an input-output relation, is used in their design or final implementation, as in [Slo87, Tsu90]. The design of this type of model based control (MBC) is usually achieved in two steps: an a priori 42 estimated dynamic model of the plant is first implemented in the control structure to linearize plant dynamics, and another controller is then designed for the resultant simplified dynamics to achieve a desired performance. The application of MBC is however hindered by a practical problem: how to determine the actual dynamics of a complicated, highly non-linear, and highly coupled robot manipulator. Some of these issues can be resolved by modeling the manipulator links as rigid bodies, however the problem of varying payloads, unknown effects of friction, external disturbances, remain. The improvements to deal with such issues have come in two broadly generalized areas: adaptive MBC and robust MBC. In the adaptive MBC structures, an adaptation mechanism is applied to capture the payload variations and compensate for the unmodeled dynamics and external disturbances. The robust MBC approaches employ a fixed-structure control algorithm in their configurations to generate a robust performance with low sensitivity to modeling uncertainties. Compared with adaptive approaches, the robust approaches are simpler to implement and require less computational load. ^- ' " The authors assert, based on the current state of control technology, that an improved controller scheme should lead to satisfying the following features: the control performance is insensitive to modeling errors and external disturbances; no high feedback gains are utilized in the design; the modification structure is simple and the performance analysis straightforward. In lieu of this assertion, the authors have presented a dual- model-based control (DMBC) structure for the trajectory-tracking control. This structure consists of two prime elements: a basic control structure and an uncertainty attenuation compensator. 43 The basic control algorithm is first applied to modify the robot dynamics so that it becomes an open-loop stable system. The authors then select the robust approach in the form of internal model control (IMC) structure to further compensate for uncertainties. It is stated that this DMBC approach is an enhanced scheme of the conventional robot MBC algorithm, with the IMC outer loop added as a complementary structure. Simulation studies were performed to demonstrate the effectiveness of this method. A 2-link (2R) manipulator was used with the basic control structure as well as with the additional compensation structure. The results showed that the effects of modeling uncertainties (comprised of parameter errors and unaccounted frictional effects) were significantly reduced by the DMBC approach. Similar results were obtained while studying the effects or external disturbances. It is noted however, that before the disturbance errors were introduced, both systems had tracking errors due to quantization error in the simulation. Of importance is the fact that the DMBC errors are 10 times those of the single controller. This is attributed to the side effects of the filter dynamics, which can be reduced by choosing a smaller filter time constant at the expense of higher sampling speed. In another approach combining path planning and tracking control of a robot manipulator, Park and Park [Par97] have suggested the use of neural networks. This approach is used to deal with the presence of system uncertainty and obstacles and employs the use of neural networks combined with a conventional feedback controller. The path planner provides not only the (sub) optimal trajectory for a given cost function but also the configurations of the robot manipulator along the path by considering robot dynamics. The path is adjusted to avoid singular points and obstacles. An additional 44 . neural controller compensates for the tracking errors caused by uncertainty and disturbances, which provides robustness with a good tracking performance. Amin and Ahtiwash [Ami96] have proposed another neural network based scheme. The authors employ two neuro-controllers utilizing the back propagation algorithm for robot tracking performance: the Inverse Neuro-Controller and the Neuro- Emulator Neuro-Controller schemes. For a given task of moving a robot from an initial rest position to a final specified position in minimum time, the profiles for position and velocity are determined. The results indicate that the two neuro-controllers allowed smooth and accurate motion of the manipulator. CHAPTER 4 PATH TRACKING METHODOLOGIES The following material presents an investigation into the development of path tracking methodologies and their application to autonomous robots, serial manipulators, and parallel platforms. The underlying assumption throughout the work is that the desired continuous path is known as a function of time, and if only discrete points along the path are known, a continuous approximation can be generated. These approaches were developed in part from inferences made from preliminary attempts at path tracking techniques that are presented in Appendix A. The results of the simulations based on these earlier approaches, while not being very promising, did provide insightfiil information. It appeared that the mechanism observed a chaotic behavior and did not converge to a solution. Although the accelerations and velocities were quite high and discontinuous, it was anticipated that the error could be reduced and the solution would converge. This unexpected behavior was quite peculiar and lead to some conjectures: • The behavior could have been the result of the method being based purely on a proportional controller type algorithm i.e., correction was attempted purely for error, and with no damping in the system, stability could have been an issue. • Another reason for this type of behavior could have been the interaction of the links of the manipulator with each other. A solution to this problem was testing the algorithm for an autonomous robot case so that there is no interaction amongst the links. Based on these observations, the basis of path tracking is developed first, followed by path tracking for autonomous robots, serial manipulators, and parallel 45 46 platforms. The developed algorithms were implemented in simulation and their results follow each section. Screw Theory Based Path Tracking The problem of a rigid body following a path with a desired pose (position and orientation) is considered first. Consider the situation described in Figure 4.1. The desired path of the rigid body B is given by the timed path of point 0, and the angle 9(t), which is the angle between the x-axis of a coordinate system fixed in rigid body B and a fixed reference. It is assumed that at some time to, the current pose of body B is given by Bc(to). It is also assumed in this case that only pose information is available. PATH Desired pose at time (to) Yc ^N°^^"""""''~-~^ ^ vA^kS"^ Desired pose at time (t„+dt) Oc(to) Xc"" '^ ..vo^^^"^ '^X Current pose at time (t„) \ Figure 4. 1 : A rigid body following a path with a prescribed pose. The problem is to devise a technique so that the error between the current and the desired poses is either eliminated or minimized to within a certain threshold. Chasles' Theorem [ChaSO] will be used as the basis of undertaking this problem. Chasles' Theorem states that given the two positions of a rigid body, there is a unique screw 47 displacement that moves the body from one position to the other. This result will be used to investigate a variety of techniques to solve the problem. The solution is first attempted without regard to time, i.e., time is of no consequence in the desired displacement. From Figure 4.1, the following positions are considered: • The current pose at time to, and • The desired pose at time to Using these two positions, Chasles theorem ensures that there is a unique screw displacement that moves the rigid body B from the current position to its desired position. Let this screw displacement be denoted by the screw, $^(, j^^^, , . Further let AG be the rotation angle and h = As/ ^0 be the pitch associated with it, where As is the linear translation along the screw axis. Finally, assume that the screw displacement will take place in a time interval At. Then, assuming that the displacement takes place at a constant angular velocity, the angular velocity associated with the rigid body (or the end effector) is given by: A0 ""^^ (4.1) Further, the constant velocity state associated with the rigid body B (or the end effector, in case of a serial manipulator) is given by: The final result of this path tracking technique is that at time to+At, the rigid body B will have the desired position at time to. Thus, timewise, the tracking will be behind the 48 desired tracking by an interval of At. Alternatively, it can be asserted that body B will track the desired path with a lag. Extending the above case and considering time as a factor, a rather simple way to avoid the lag or time delay of the previous technique is as follows: • The positions to be considered are: The current pose at time to and the desired pose atto+At. • To require that the resulting screw displacement will take place in the time period At. The first condition will determine a different screw, $c(,j-></(,„+a/) » ^ different rotation angle A9, and a different pitch h=As/A9. The second condition would yield that the constant angular velocity, associated with the screw is, and the constant velocity state of the rigid body (or the end-effector for a serial chain) is given by ^ = «$c(,„w(,„.A/) (4.4) This technique ensures that at time to+At, both the current and the desired positions coincide. Whatever the employed technique, if the rigid body B is the end-effector of a non-redundant spatial serial chain, ybr example, the corresponding joint velocities can be computed as follows (Figure 4.2): A closer look at these techniques obviates the fact that these techniques would produce commands 49 • with discontinuous angular velocities and • with discontinuous locations of screw axes. Figure 4.2: A serial manipulator traversing a prescribed path. These discontinuities will produce infinite accelerations, and obviously undesirable characteristic for any robotic system. Additionally, there is another quite subtle source of a problem. It should be noted that in the case of a serial manipulator, Equation (4.5) can be used to calculate the joint velocities ,6>,+i,(/ = 0,1,2. .,5) . However, in the time interval At, there will be position changes that will affect the screws '$'^'. Thus the joint velocities are only accurate at t==to. This however, might not be a major source of problem as the transient error may be reduced to an acceptable threshold by varying At (making it smaller). 50 Autonomous Robot / Vehicle Considering Pose Information Only ; Now, the problem of an autonomous robot following a path with a desired pose (position and orientation) is considered as shown in Figure 4.3. The desired time dependent path, ( Xj(t),yj(t);0j{t) ) , is assumed to be specified. The current pose of the body at the instant /p, is given as (x^(/o),>'^(/o);^^(/o))and the desired pose at time (t, + At) is ( X, (fo + AO, yj (to + A/); e, (/o + A/) ) . Y A OAto + ^O X Figure 4.3: Autonomous robot path tracking with pose constraints. Considering the position level only, the following desired velocity screw can be derived (from Equation 2. 39): 51 V = (4.6) where, 0),= OAL + AO-e^it,) At ^.= ""y^ At yAto + ^t)-yAto) At (4.7) The resultant motion due to this screw will ensure that at time (t^ + At) the position and orientation (pose) will coincide with the desired pose. It should be pointed out here that this method assumes that all velocity screws are possible, which may not be entirely feasible due to vehicular movement constraints. It should be noted that at the pose level, the commanded motion is continuous at the position level and is a linear approximation of the trajectory. Considering Pose and Velocity Information In this approach, the desired velocity state is also specified in addition to the pose information. Initially, it is assumed that current pose and velocity information is known. As shown in Figure 4.4, the current state is defined by: ^c(^o).>'c(^o);^c(^o) 1 t^.c(^o).'^^c(^o);<yc(^o) J and the desired state at instant {t^ + At) is: 52 -» Using the pose information, the velocity screw, V (4.6), as done previously, can be calculated as : a>. V, ""y- At At At (4.8) Further, from the velocity information, it is possible to determine the following: f//o + AO r,(/o+AO \ -^ ► X Figure 4.4: Autonomous robot path tracking with pose and velocity information. «, a, - S = 53 o),it,+At)-co^{t,) At ^..(^o+AO- -^A(o) At u^,{t,+At)- -OyA(0) At (4.9) This information, coupled with the velocity state at t^, will provide a reduced acceleration screw A^ . However, it is important to note that determining how the two screws, velocity and acceleration, will affect each other is an issue that needs to be addressed. The problem is now set up differently in a way conducive to determining the common action of two screws. Assume that the initial or current pose information (state) of the body is known. Consider a rigid body B and let (l»^o,l>_^o;«o) denote the initial or current velocity of the body and let (iJ;fOc'^>'Oci^oc) represent the (to be determined) required command velocity parameters. Also let (a^,ay;a) denote the acceleration state. Then after a time step, At , the final velocity and position is given by: Velocity: cOf =(0q+ aAt (4.10a,b,c) Position: Oj =0o+o)oAt + ^aAt^ yd =yc+Oyo^t + ^ayAt^ (4.11a,b,c) 54 These equations above are the algebraic equivalent of the common action of a velocity and an acceleration screw. It will now be shown how the command parameters (D^oc'^rOcJ^oc) will be calculated. Of course, the calculated command parameters may differ from the current velocity parameters, and any control system that is applied will act to minimize this difference. Assuming that the initial pose (j^o»>'oi^o) '^ known and the final pose (xj^,yf;0j^), the final velocity (v^Qf,Vyf;o)f), and the time interval At are specified. Equations (4.10) and (4. 11) above are used to solve for co^^ and a as follows: From Equation (4.1 0)a we have, a = At substituting this in Equation (4. 1 l)a gives. (4.12) 0f^0o+(OoA( + l At' rearranging, we obtain Oj^ -6q =cOo^At + -^o)j-At—^o}Q^At = jCOQ^At + jCOj^At and finally solving for cOq^ , 2(0,-0,) 6>n. = (0 ■'Oc At f (4.13) where cOq^ is the velocity for the time step At that would satisfy the pose and velocity requirements at t + At . To solve for a , substituting Equation (4.13) in Equation (4.12): 55 a (Oj_ 1_ 2(0,-0,) At -co, a = At «/- 2 (0^0,) At + CO, a -■ 2{ {0,-0,)\ At 0), A^ (4.14) The linear velocity and acceleration is calculated by following a similar procedure. Considering Equations (4. 10) b,c and Equations (4. 1 1) b,c respectively: a, = At At Xd=X^+^xOc^( + \ yd^yc+^yoc^(+ At At' ^l^yf-^yOc^ At At' In a procedure analogous to the procedure for determining co,^ and a above, the follov^ing unknowns are determined: V l\x , Xq ) 4:0c ^^Oc = A^ -V xf 2(>'/->'o) A^ V yf (4.15) (4.16) and <^x = At V a,, = — ' At 4 ""yf- (^/-^o) ' At J AX ] (4.17) (4.18) Once all the unknowns are determined, the desired velocity state and the acceleration state of the body are known: 56 V.= "0 r "" (^x «</ , d = , 5 = fl. ^.d a y ^yd L _i (4.19) and the reduced acceleration screw (the reduced acceleration state) is calculated as: A = a Uq- COXUq (4.20) It should be noted that when pose and velocity information is used, the commanded motion is continuous at the pose and velocity level, which implies that the commanded motion is a quadratic approximation of the trajectory. The results obtained from the analysis were employed for a simulation. The simulation was done in MATLAB®. The test cases and the results obtained are presented next. The developed algorithm was tested with two cases. In both cases the desired path was a circle. The idea behind using a circular path was that for such a motion, the instantaneous screw axis(pole) would be the center of the circle and would provide a means of determining an error; which otherwise would be difficult as mixed angular and linear units are involved. Using Equations (4.6) and (4.7), the constant angular velocity is determined to meet the pose tracking criteria. From that information, the location of the pole or ISA is determined using _ _ (OXU, r =r--\ m (4.21) In the first instance (autonomous.m. Appendix B), the center of the circle was located at (1,0) and the starting point for the robot was(1.51, -.05,0). The results of the 57 tracked path are shown in Figure 4.5a. The axes of the graphs represent the x and y directions of the plane. The 'o' legend in the center of the circle represents the pole locations of the instantaneous screw axis at each interval of time A/ . Figure 4.5b shows a zoomed-in version of the pole locus. It is interesting to note that the root locus is a circle too as opposed to being the center point of the circle. This discrepancy is attributed to using a difference formula for determining the angular and linear velocities. In the second instance (autonomous l.m, Appendix B), the circle was located at (1,0.5) and the initial position was specified as (1.5, 0.5). The results are shown in Figure 4.6a and 4.6b respectively. In the second case when both the pose and desired velocity of the robot is specified. Equations (4.13) through (4.18), which require pose and velocity state information, are used to determine the angular velocities and constant angular Path Tracking (pose) : Screw Locations "o" 0.6 0.4 0.2- ff- -0.2 -0.4 -0.6 /' \ . \ Start .1 / ^++^ ■++. +++-^f4+++++4+++'"" .++-" o pole locations + path 0.6 0.8 1 1.2 1.4 1.6 1.8 2.2 2.4 Figure 4.5a: Autonomous Robot: pose information only. Tracked path and pole locations. 58 accelerations to meet the pose and velocity tracking criteria. Again, the location of the poles or ISA's are determined using Equation (4.21). Two paths were considered in the simulations. In the first instance (autonomous2a.m, Appendix B), the path was a circle with radius 0.5 centered at (1,0.5). In this case, the orientation was also variant, i.e., the desired orientation was set to be tangent to the circle at every instant. The starting position of the robot was set arbitrarily at (1.54, .45). Figure 4.7 shows the path tracked and the locus of poles. The first pole appears at quite a distance from the rest of the poles, due to the fact that the starting position of the robot is off from the desired position. Also shown in the figure is the position and orientation of the robot at each time period. The second case (autonomous3.m. Appendix B) that was simulated involved tracking a parabolic path. The starting point of the robot was set at (-0.45, 0.81). The desired orientation changed as a function of time by (5 x A/ ). The results are shown in Figure 4.8. 0.01 0.005 -0.005 Path Tracking (pose) : Screw Locations "o" 1?^ '^T?;!: O ; O ; O I o ;■■■■ o o O I o o o o ••& f- o o o o o o o o O ; O : -0.01 f- On 0.985 ^ o pole locations ~ path o o o I o I o ■o- o o o o o o o o o o o o o o o o o j" ■^ 0.99 0.995 1.005 1.01 1.015 Figure 4.5b: Autonomous Robot: pose information only. Pole locations. 59 Path tracking (pose) : Screw Locations "c )" 1 p ' + + + " ""^ + + + ' o pole location " + , + path 0.9 \ " 0.8 ■ * + + + 0.7 + + l + + + + + + + M + + ■•«*■: ,... + + 0.5 + + + • Start^,^-^ + ■ ■ ■ ■ ■ ■ + 0.4 + - + + + + 0.3 + + + + 0.2 0.1 y - 1 "^++ + ++-a+-t-±i±l — 1 1 1- 0.4 0.6 0.8 1.2 1.4 1.6 Figure 4.6a: Autonomous Robot: pose information only. Tracked path and pole locations. Path tracking (pose) : Screw Locations "o" 0.515 0.51 0.505 0.5- 0.495 0.49 0.485 - : ooooowooc o pole location ^ path 0.985 0.99 0.995 1 1.005 1.01 1.015 Figure 4.6 a: Autonomous Robot: pose information only. Pole locations. 1F r- 0.8 0.6 0.4 0.2 -0.2 60 Path tracking (Pose, Velocity) : Screw Locations "o' jfo pole locations ^^:^r — '^ej, 'ejsi -<^ 0.2 0.4 0.6 0.8 1.2 1.4 1.6 Figure 4.7: Autonomous robots, pose and velocity information. Poles, tracked path, and vehicle orientation (box). Path Traclflng (Pose, Velocity) : Screw locations "o" 1.8 1.6 1.4 1.2 1 0.8 0.6 0.4 0.2 - T T f 1 1 1 1 1 pole locations path - o - o . ■'•' <i> start .'~^ o ° <:> & ° ■ ° ^ o o " Q ° o D - \1 w°° JJ M 1 L. J 1 1 1 1 1 1 1 -0.8 -0.6 -0.4 -0.2 0.2 0.4 0.6 0.8 Figure 4.8: Autonomous robots, pose and velocity information. Poles, tracked path, and vehicle orientation (box). 61 3R Planar Manipulator Considering Pose Information Only A new approach to the tracking analysis for a 3R Manipulator is developed (as opposed to the approach discussed in Appendix A). Consider the 3R Planar Manipulator shown in Figure 4.9, with the pose information available, i.e., the current and desired pose states are known. The approach utilizes the inverse position analysis of the manipulator to determine the current joint angles for position, 6^^ , and the joint angles for the desired position, 0^j . From the results of the analysis, the joint angular velocity is calculated by using the formula (Equation (4.7)): ^,,(/o+AO-^,c(^o) <y. = At (4.22) Desired Path Figure 4.9: 3R Manipulator with pose information. 62 To determine the errors in the desired and commanded velocities, the desired velocity state is determined from the path of the manipulator. Also, from this desired velocity state, the joint angular velocities are calculated by using the following formula, the underlying assumption being that the path is continuous and differentiable: QQ)^°$^+^Q)2^$^+20)J^S^ =V (423) [j]o) = V (4.24) ^ = UV V (4.25) where, \j\ is the Jacobian of the manipulator, <» is a vector defining the joint angular velocities of the manipulator, and F is a vector of the velocity state of the manipulator. Path tracking of a 3R planar manipulator was simulated using the current and desired pose information using this methodology. A flowchart of the algorithm is shown in Figure 4.10. All the link lengths of the manipulator were set as unity. The desired motion of the manipulator was a circle specified by (0.5 + r cos(0,A'sin(/)). The orientation was not varied. The algorithm (main4.m, Appendix B) uses the inverse position analysis to determine a set of joint angles. These joint angles are then used to compute the command angular velocities. To determine the effectiveness of the algorithm, the calculated velocity parameters (command angular velocities) are compared to the velocity parameters that are determined using infinitesimal motion screws (desired angular velocities). This provides a rough order of magnitude comparison and validation of the command screw. The algorithm computes the Jacobian of the manipulator, which coupled with the information about the velocity state, provides the desired joint angular velocities. The two values are compared to determine the error (Figure 4.10). 63 To illustrate how the algorithm works, an iteration of the procedure is shown from the algorithm main4.m. The current pose or state of the manipulator is given as r, =(2,1,0) , <l>,=0 where (j) is the orientation of the end effector. The desired path and velocity for the manipulator is specified as (a circle with radius 0.2 and the center at (0.5, 0)) r^ = (0.5 + 0.2 * cos{t + At), 0.2 * sin(/ + At), O) , ^^ = ^4 25) Vj - (- 0.2 * sin(^ + At), 0.2 * cos(/ + At), O) . (4 27) Inverse position analysis is performed by the function reverse_position_3R at the current pose at time t and at the next desired pose at instant t + A t . The inputs to the function are the link lengths of the manipulator and the current position of the end effector. The outputs of the function are the joint angles of the manipulator and the Jacobian matrix (as defined in Equation (2.49)). Using the joint angle information at instant t and t + A t the command velocity screw for each joint is generated using Equation (4.22) as ^command -^^—' (4.28) The Jacobian matrix is used to determine the joint angular velocities as dictated by the desired velocity state, F^ , by using Equation (4.25) as CO. -Wy. calculated L J d (4.29) The two joint velocities are then compared to verify that the generated command satisfies the desired velocity constraints. The results indicate that the command velocity is within the desired velocity range by an average of 0.006% with a standard deviation of 0.02rad/sec. ^ 64 Figure 4.11 shows the manipulator and the traversed path. Figures 4.12a and 4.12b, and Figures 4.13a and 4.13b show the actual and desired angular velocities and the errors respectively. The errors are high at the initial step as the algorithm attempts to jump to the desired position in a single step. This can be addressed by dividing the motion of the first step into smaller increments or by considering reducing the error in more than one step, for example, in Ibd instead of A/ . It should also be noted that this step is application dependent; i.e., this issue is generally avoidable if the path is pre- planned and the initial starting point for the manipulator is known. ■ . Given the continuous path (desired position and orientation) Perform inverse position analysis at current and desired pose 9's: current and desired are known Determine the command joint angular velocities that satisfy criteria at the pose level Determine the joint angular velocities using the velocity state and the inverse velocity analysis Compare results. -*{ The difference between terms is the \*- control error. Figure 4. 10: Algorithm summary for 3R Manipulator path tracking using pose information. 65 Tracking of 3R manip. using inv. analysis (Pose) Figure 4.11: 3R Manipulator path tracking with pose information. Manipulator and the traversed path. tllTW 40 Actual and Desired Angular Velocities 1 — omegaOl omegaOl J 30 — omega12 omega 12|, 20 1 ■■ ,.;.- , ' - — omega23 omega23j 10 10 - 1 - 20 - - 30 I : - 40 - 50 fin I r 1 1 r 1 Figure 4. 12a: 3R Manipulator path tracking with pose information. Actual and desired angular velocities. 66 Actual and Desired Angular Velocities 0.6 ' l\i'' 1 1 1 1 1 1 omegaOl omegaOIji — omega 12 _ omega12j l\ 0.4 0.2 : ■ — omega23 omega23u ■ ■ ■/ ^ 11 -0.2 1 -0.4 -0.6 -0.8 1 1 \ 1 1 1 1 1 1 1 1 1 1 ~ 0.18 0.19 0.2 0.21 0.22 0.23 0.24 0.25 0.26 0.27 0.28 time Figure 4.12b: 3R Manipulator path tracking with pose information. Actual and desired angular velocities (magnified view). Errors in Joint Angular velocities error wOl — en'orw12 15 10 - en-or w23 -' 5 ■ -5 - -10 - - ■ -15 -20 , \-:' r \_ ■.« ' 'i .-:■ JK; - '*. ■ ■■ -25 30 1 1 1 t 1 1 12 3 4 5 6 7 time Figure 4.13a: 3R Manipulator path tracking with pose information. Errors in joint angular velocities. 67 0.04 Errors in Joint Angular velocities 1 ' ■ - ' V I 1 1 ■ 1 1 1 0.03 — error w01 --- error w12 error w23 . 0.02 - 0.01 . - - - -0.01 - - -0.02 - - -0.03 ' -0.04 •' - -0.05 1 1 1 1 1 1 1 1 1 0.198 0.2 0.202 0.204 0.206 0.208 0.21 0.212 0.214 time Figure 4.13b: 3R Manipulator path tracking with pose information. Errors in joint angular velocities (magnified view). Considering Pose and Velocity Information The above analysis is further extended to consider the case where the desired velocity state is also specified as a constraint (Figure 4.14). The analysis is done along the same lines as for the autonomous case in the previous section whereby the common action of the acceleration and velocity screw is determined. In addition to determining the joint angular velocities by using Equation (4.13) '»'=^^-' Af (4.30) the joint angular accelerations are generated from (Equation (4.14): 68 0Ato + ^t) Desired Path Figure 4. 14: 3R Manipulator with pose and desired velocity a. = At 0)^ 6i (4.31) In a way analogous to the pervious section, in order to determine the errors in the desired and calculated accelerations, the desired acceleration state is determined from the velocity state of the manipulator (again, the assumption being that the velocity function is continuous and differentiable). From the desired acceleration state, the joint angular accelerations are determined as follows. The equation determining the reduced acceleration state of a manipulator is: Ar = ,<y,°$'+ ,ft),'$^ + 2<y3^$^ + 0"'l "J- f i"'2 Ocl ,- Ic2 , „ 2(ij3l 0)^ $ +2^3 ,«/2 + (4.32) .0^1 $ where <y 's represent the joint velocities, the cb's represent the unknown joint accelerations, and $ are the screws that comprise the Jacobian. Once the reduced acceleration state is known, the joint angular accelerations are determined: 69 The above equation can be written as which in turn can be written as: [J] Q} + [L] = A^ (4,34) [J]d) = A„-[L] (435) and finally solving for the acceleration, ^ = [Jr[A,-[L]] (4.36) where [L] is the term containing all the Lie products. In this case, the path tracking of a 3R manipulator was simulated with the desired velocity added as an additional constraint. The algorithm generates the command velocities and accelerations by using the position and desired velocity information. For comparison, the algorithm calculates desired acceleration state (from the desired velocity) and determines the desired joint accelerations. These are compared to the command velocities and accelerations to determine how closely they follow. The algorithm flowchart is shown in Figure 4.15. As in the previous case, the link lengths of the manipulator are set as unity. Two different cases were considered. In the first case (Case I), the path to be tracked was the same as the one considered for the pose-only case (main54.m, Appendix B). This was done so that the results could be compared. The path simulation is shown in Figure 4.16. Figures 4.17 through 4.20 show the angular velocities and accelerations and the errors. A second case (Case II) was considered (mainSa.m, Appendix B) in which the path was kept the same but the orientation of the end effector was varied as it traversed the path. This case is illustrated in Figures 4.21 through 4.25. 70 Given the continuous path (desired position and orientation) and the desired velocity state. Perform inverse position analysis at current & desired pose 6's: current and desired are known Determine the command joint angular velocities and accelerations that satisfy criteria at the pose and velocity level Determine the joint angular velocities and accelerations using the velocity state, and the acceleration state Compare results. The difference between terms is the control error. Figure 4. 15: Algorithm summary for 3R Manipulator path tracking using pose and desired velocity information. 71 Tracking of 3R manip. using inv. analysis (Pose, Velocity) Figure 4.16: 3R Manipulator path tracking, using pose & desired velocity information. Motion of the manipulator and the traversed path. Case I. -2 -8 Actual and Desired Angular Velocities -•'-/.AI^T?- tinne omegaOl omegaOlj omega 12 omega12j omega23 onriega23^ Figure 4. 1 7: 3R Manipulator path tracking with pose and velocity information. Actual and desired angular velocities. Case I. 72 6? ■\ q" Errors in Joint Angular Velocities o 1 1 1 1 1 1 — error wOl - en-orw12 5 en-or w23 . ' 4 ■ 3 ' ^ -. 2 ■ ,■ 1 , A _ ■'I 1 1 1 1 1 1 1 2 3 4 5 6 ■ ■ time Figure 4.18: 3R Manipulator path tracking with pose and velocity information. Errors in joint angular velocities. Case I. '1'^n Actual and Desired Angular Accelerations I3U 1 1 1 1 1 1 — alphaOl alphaOl^ 100 50 i / ' - - alpha12 alpha12j alpha23 alpha23^ 1 - C 1 r.^,l- i •. -50 i. ;^ -100 ! ■150 1 1 1 1 1 1 12 3 4 5 6 7 time Figure 4.19: 3R Manipulator path tracking with pose and velocity information. Actual and desired angular accelerations. Case I. 80 60 40 20 -20 -40 -60 -80 73 Errors in Joint Angular Accelerations en-or alphaOl en-oralpha12 error alptia23 3 4 time Figure 4.20: 3R Manipulator path tracking with pose and velocity information. Errors in joint angular accelerations. Case I. Tracking of 3R manlp. using inv. analysis (pose, vel.) 0.8 0.6 0.4 0.2 0- -0.2 -0.4 1 1 1 1 '41 ' 1 1 — desired path \/ y i f ^#MX jM ^^^W 1 V ■ \\ 'm^^k m 1 . 'iwyY^A/m K\\\\ i ■ - mX/vW ^xM ^^ -^^*t=fc=~v IiXXa-A •%' Y'f~ __p^± ~^\ 'ii \\^^r\\\v^ — ~*^^ ^^^=r- \ ^^^^ ^ in^^^^^^^— --7 +^^^ ==^ ^s^ ^^^=^^^^== 1 1 1 1 1 1 1 -0.8 -0.6 -0.4 -0.2 0.2 0.4 0.6 0.8 Figure 4.2 1 : 3R Manipulator path tracking, using pose & desired velocity information. Motionof the manipulator and the traversed path. Case II. 4- -6 74 Actual and Desired Angular Velocities QA 1.5 time — omegaOl omegaOlj omega 12 __ omega 12|j — omega23 omega23^ Z5 Figure 4.22: 3R Manipulator path tracking with pose and velocity information. Actual and desired angular velocities. Case II. 25 x10"" Errors in Joint Angular Velocities en-or wOl -- en-or w12 - en-or w23 2 1.5 - ' * : - 1 - j 'm 0.5 - ; ■"? - : : \ ■ r ■ ^, ■ ' \/ 0.5 .1 " — .1 1 1 1 - 0.5 1.5 time 2.5 Figure 4.23: 3R Manipulator path tracking with pose and velocity information. Errors in joint angular velocities. Case II. 75 1'^n Actual and Desired Angular Accelerations ov 1 1 1 1 alphaOl alphaOl^ 100 50 -50 — alpha12 alpha12j alpha23 alpha23^ ■\ 1 ■' - -100 1 en 1111 ■ 0.5 1 1.5 2 2.5 time Figure 4.24: 3R Manipulator path tracking with pose and velocity information. Actual and desired angular accelerations. Case II. fin Errors in Joint Angular Accelerations 0\f 60 40 1 t 1 ' — en-or alphad — eaor alpha 12 en-or alpha23 '-'■•- 20 -20 ■"",'. _ ■'-"'■'"■ " ■ '■] ■" ■ -40 ■■• ■ -60 80 1 1 1 1 0.5 1 1.5 2 2.5 time Figure 4.25: 3R Manipulator path tracking with pose and velocity information. Errors in joint angular accelerations. Case II. ' 76 Parallel Manipulators Parallel mechanism based approaches to manipulator design have grown in popularity in the past decade or so. Mechanisms for a variety of applications including robotics, machine tools, measurement machines, and sensors have been developed based on parallel devices. A parallel mechanism or a platform is defined as any mechanical device that has legs (connectors) that connect a moving platform to a base. This type of mechanism possesses desirable characteristics of high accuracy, high payload-to-weight ratio, and good static stability. The geometrically simplest parallel mechanism has the structure of an octahedron and it is designated as a "3-3 platform" since there are three connecting points on the base and the top platform respectively. This geometry while being simple, has a serious mechanical disadvantage as it is very difficult to design concentric ball and socket joints at each of the double connection points on the octahedron without mechanical interference. To avoid the shortcoming of this platform, platforms with different geometry have been developed, some of which are the 6-3 platform [Ste65], the 6-6 platform (hexapod), and the special 6-6 platform [Gri93]. To control these mechanisms it is necessary to determine accurate compliance models. These kinematic models can be obtained if the position and orientation of the moving platform is known relative to the base. This is known as the inverse kinematic analysis for the system. The complement of the inverse analysis is the forward analysis, which entails determining the position of the moving platform with respect to the base given the six connector lengths. For a general 6-6 platform, the closed form forward analysis equation is a 40'*' degree polynomial [Rag93], which is computationally impractical for real time control. On the other hand, the forward solution of a 3-3 77 platform involves solving an eighth degree polynomial in a single variable. The special 6-6 platform provides combines the benefits of both of these platforms. These platforms allow for the simple analysis of the 3-3 with an eighth degree polynomial, and allow for the general benefits of the 6-6 by eliminating mechanical interference. Based on the combination of these advantages, the special 6-6 platform has been a subject of research at CIMAR for a variety of applications; among them is the development of a force-feedback end-effector, as well as the study of the platform as possible machine tool for milling applications [AbbOO]. Currently, the platform is being used as the basis for development of a Weight Compensating Parallel Manipulator (spatial jack) to handle large payloads. Tracking Methodology The path tracking methodology developed for parallel manipulators involves the use of the inverse position and the inverse velocity analysis. The length and the velocity of each connector are then determined. The initial attempt to solve this problem was by assuming an acceleration profile that was linear, i.e. the variation in the acceleration was linear (Appendix A). The results obtained from this analysis indicated that this assumption would not yield a result that would satisfy the position, velocity, and acceleration specifications at the end of each time step. To overcome this problem a different approach was attempted which is discussed below. To meet the path tracking criteria, an approximation was required that would produce the desired motion performance at the beginning and end of each step. Since the criteria for motion planning were assumed to be position, velocity, and acceleration, these criteria involve six conditions at the beginning and end of each step. These conditions 78 are the initial and final position, velocity, and acceleration of the actuators, respectively. Thus the approximation is based on a fifth order polynomial that has 6 constants. The 6 constants match the number of conditions required to satis fy exactly the position, velocity, and acceleration state at the beginning and end of a time step. Consider the polynomial S(t) = Qq + ^1^ + ^2^^ + ^3^^ + '^4^^ + ^5^' (4.37) differentiating with respect to time 5(0 = a, + 2a2t + Sflj/^ + 4a^t^ + Sa^t"* (4.38) and again s'{t) = 2^2 + 6a^t + lla^t^ + 10a/ . (4.39) For / = , and 5(0) = 5o siO)^s, s'(0) = sl subsfituting in Equations (4.37),(4.38),and (4.39), yields ^0=^0 . '.y (4.40) s,=a, (4.41) sl-2a2 or (4.42) ' 2 (4.43) Substituting the known coefficients above in Equations (4.37),(4.38),and (4.39), we obtain s(t) = So+ SqI + ^sy + a/ + a^t'^ + a/ (4.44) s\t) = 5o + sit + 3a/ + 4a/ + 5a/ (4.45) 79 s\t) = sl+6ajt + \2a^t^ +20a^t^. (4.46) At the end of the time step At , the first, second and third order equations become Sj- =Sq+ SqAI + jslAt^ + a^At^ + a^At* + a^At^ Sf^SQ^-slAt + 2>a^At^+Aa^At^+5a^Ai* s'j -sl+6a^At + \la^At^ +2Qa^At^ . Rewriting the equations above and substituting d for At for the sake of clarity, s^-Sq+ s\6 + ^s'qS^ + a^S^ + a^5'^ + a^5^ (4.47) j^ = 5o + 5o^ + 3<33^^ + 4a4^^ + Sflj^" ' (4.48) s'j^=sl+6a^5 + \2a^S^ +20a^S^ . (4 49) The equations above have three unknowns, a^, a^, and a^ . These equations were solved using Maple®, and yielded the constants as hsl5'' + 205, - 205„ -\2s'8 + s\5^ - %Sr5 ' 2 > V Qiy -r jj^L 'r a, = Gs'qS^ +I5sf -15sq -SsqS + s'j^5^ -Is'^S 1 f5sl5' + 125 . - 125o - 6slS + s\5^ - 6s' 5^ (4.50) (4.51) (4.52) Once all the coefficients {a^, 02,0^,0^,0^) are known. Equations (4.37), (4.38), and (4.39) can be used for generating commands that satisfy the desired conditions for position, velocity, and acceleration. 6-6 Parallel Manipulator The parallel mechanism considered as an example is the special 6-6 parallel platform. The reason for considering this platform is the availability of a design methodology for designing such a platform to suit a particular application. The 80 parametric methodology developed by Abbasi et al. [AbbOO] is used and is concisely presented here. Generally, when designing a platform, the anticipated application must be considered. The application defines the loading conditions, workspace geometry, and dexterity, based on which the platform parameters are optimized. The number of design variables for the special 6-6 platform is many. To examine various designs in a proficient manner a methodology has been developed that reduces the design variables. The platform kinematics can be described parametrically by defining two triangles, the base and the top, and six side pivot connections. An equilateral triangle is used as provides symmetric stiffness and response over the workspace. The equilateral triangle can be defined by one parameter, the side of the triangle. The top triangle is related to the base triangle by the parameter tsidescale. The parameters bscale and tscale define the base and top side pivot points, respectively. Another parameter, hscale, is used to define the perpendicular distance between the top and the base of the platform. This parameter is also a function of SIDE. If hscale is equal ^. ^ ^^ r^ ■ . • .- Figure 4.26: Design parametenzation of the special 6-6 platform. 81 to 1, the distance between the top and the base of the platform is equal to SIDE, and a variation of this parameter would vary the height of the platform accordingly. The special 6-6 parallel mechanism requires that the pivot connection lie on the line that contains the two vertex connections. For example, in Figure 4.26, side pivot connection 2 must lie on the same line that connects 1 and 3. On the base, if bscale is less than 1, then the side pivot lies between the two vertex connections and vice versa. The same is valid for tscale with respect to the top. A given platform may have a large kinematic workspace, but its stability over the workspace may vary enough to have affect on its performance. The parameter SIDE allows the mechanism to be scaled up or down. The other parameters define the kinematic workspace and variation of static stability across the workspace. This methodology allows finding suitable parameters that define a platform that is well behaved over the workspace. The inverse displacement analysis is useful in developing the expressions for the dynamic state of the system. The platform location must be specified for a particular task. The displacement and orientation of each of the connectors that actuate the platform can then be determined once the platform's motion time history has been specified. From the motion specification, the velocity and the acceleration state of the platform is computed and subsequently, the actuator speeds and accelerations are determined. The analysis is further extended to determine the forces in the actuators. Algorithm Implementation The objective of the algorithm is to provide coefficients for the command equations (4.37)-(4.39)developed two sections ago. Another objective of the algorithms is to provide feedback in terms of the leg velocities and accelerations, and leg forces, to 82 , enable the designer to check the viability of the commands; i.e., by getting the feedback it can be determined whether the hardware would support and be able to match the commands (generally, a velocity command with limits on velocity and acceleration). Initially, an algorithm (Movplatlc.m, Appendix B) was implemented, which when given a desired pose, velocity, and acceleration, would compute the position, velocity, and acceleration of the connectors using the inverse analysis (reverse66.m. Appendix B) and subsequently would generate the coefficients for the command equations to meet the position, velocity, and acceleration conditions. For this case, the platform design parameters were taken as follows: SIDE = 30 inches bsacle = O.S tsidescale= 1.0 tecfl/e = 0.8 I ,,..■''■ hscale = 1 .0 and the desired motion was a pure rotational motion defined by the motion of the center point of the platform about the z-axis (vertical). The rotation was described by the Leg Displacements of the Ptatform Figure 4.27: Leg Displacements for the 6-6 platform, executing a pure rotation. 83 following rotation matrix: R = cos(5A0 -sin(5A0 O' sin(5A/) cos(5A0 1 The algorithm computed the six leg displacements, velocities, and accelerations. Based on this information, the coefficients were calculated for each leg from Equations (4.40), (4.41), (4.43), (4.50), (4.51), and (4.52). Figures 4.27 through 4.29 show the leg displacements, leg velocities, and leg accelerations of the manipulator. This analysis was further extended to determine the forces in the platform actuators. Using the concepts of dynamic analysis of rigid bodies using Screw Theory facilitated this. The equilibrium equation for the platform can be written as [J]f,ess= W (4.53) where [j] is the Jacobian composed of the screw coordinates of the platform actuators, fie& ^r^ the forces in the legs and W is the wrench on the platform. The wrench on a rigid body can be determined as W ^ I A^ + [V IV] where / is a 6 x 6 inertial matrix and is composed as follows (4.54) '6x6 -MR MI I '3x3 d MR (4.55) MR is a coordinate transformation for different reference frames, and for this particular case, is a 3x3 matrix of zeroes. MIj is the mass matrix times the identity matrix which produces a diagonal mass matrix. And finally, the 73^3 is the inertial matrix given as 84 Leg Velocities of the Platform 0.5 -0.5 1 1 1 r- -1 — - LegKD ^ ""^ Leg2(q) Leg3(m) Leg4(r) y^ '.' Leg5(n) / V ^ V V V Leg6(p) Vj / y V ^ ^ / V / - / ■• . . V / / .'< V / / ^ ., ' ; .■ -i / / V •' ■ ■ ■ ■ ' / / y< V / / V ^ / / / V-' '^ '^' ^^■ . / / 1 / V "^ / X V / x^ ^'^ ^ ' / ^ ^ / / V f^ f / — / ^5K , " / o-< - - • . / V ^ -~^^ £. A £i ^ ,,' ^ ^ ^ ^ ^ x' ^^ v" 1 1 1 I 1 0.1 0.2 0,3 time 0.4 0.5 0.6 Figure 4.28: Leg velocities for the 6-6 platform executing a pure rotation. Leg Accelerations of the Platform I' V V V V Leg1(l) -" Leg2(q) Leg3(m) Leg4(r) V Leg5(n) Leg6(p) 0.1 0.2 0.3 time 0.4 0.5 0.6 Figure 4.29: Leg accelerations for the 6-6 platform executing a pure rotation. 85 '3x3 I^ I. I J I. iyy J. I. I. ^J (4.56) Further, A^ is the reduced acceleration state and V is the velocity state of the top platform and are determined as A, = a (4.57) V = 0) Un (4.58) And finally, the term in the square brackets, [V IV], is the Lie product of two screws and is determined as shown in Equation 2.55. Once all the components are known, the forces in the actuators are calculated from (4.53) as //...= W^- (4.59) This procedure was implemented using data for a platform being developed at CIMAR as a force compensating manipulator device. The mass and inertial matrices were formed from this data. The mass matrix was found to be MI, = 79.5928 79.5928 79.5928 kg. (4.60) and the inertial matrix was calculated as '3x3 3.39146 3.39146 6.59053 kg.m (4.61) 86 The function that calculated the inverse position, velocity, and acceleration for the previous case, was further developed to include the force analysis (reverse66f.m, Appendix B). This algorithm was tested for a variety of cases, such as constant velocity motion (translation), translation with constant acceleration, rotation with constant angular acceleration, etc. The results from two cases considered are presented: • In this case, a constant acceleration motion was implemented as the platform moved in the vertical direction; results are shown in Figures 4.30 through 4.33 (Movplat2b.m, Appendix B). • A rotation of the top platform about the center (z-axis) with no translation was implemented, with constant angular acceleration; results obtained from this simulation are presented in Figures 4.34 through 4.37 (Movplat2d.m, Appendix B). Techniques were developed in this chapter for generating command screws that meet the desired position and velocity specifications for autonomous robots, serial manipulators, and parallel manipulators. The following chapter presents the case of a serial manipulator with a control system implementation. 87 Leg Displacements of the Platform 0.95 0.9 0.85 E 0.8 0.75 0.7 0.65 0.8 1 - LegKD Leg2(q) Leg3(m) Leg4(r) " Leg5(n) Leg6(p) - ^^_v_„_^^^-^-v-^^-?-V'V-v-^-?-v-^-:--?-v-^-^"^ - 1 1 1 1 1 - 0.1 0.2 0.3 time 0.4 0.5 0.6 Figure 4.30: Leg displacements for a platform motion with translation and constant acceleration. 0.06 0.05 0.04 I 0.03 0.02- 0.01 Leg Velocities of ttie Platfomfi 1 1 1 1 r LegKD ^ Leg2(q) .■.■'■'.■ a' Leg3(m) Leg4(r) V Leg5(n) - A Leg6(p) A ■ '-X A7 A ' - V A- ■ X . - .X A J/ y'- ii ^ ^'^ /■ ^'^^" - ■^ ^ ^" A y X A- .^ --^ .■• J^ -■' A J^-^y-- . : ■ A-- .J/^" .- ^^'' .-^r^" ■ _ X^ '■ V?"^ • x^^ ■ -'^ — '- 1 \ 1 1 1 0.1 0.2 0.3 time 0.4 0.5 0.6 Figure 4.3 1 : Leg velocities for a platform motion with translation and constant acceleration. 88 0.12 0.115 11 Leg Accelerations of ttie Platform - ^...^..^..A---A---A--A"A"-A-A--A--A-A.-A...A-A"A--'.a'--A"-'£'--'^--^--^- '^^ Legl(l) — Leg2(q) Leg3(m) . -- Leg4(r) V Leg5(n) Leg6(p) 0.105 1 °' 0.095 - ■ - - . 0.09 _ ■ 0.085 . ^^_^,^.^^^-w-v-^-.-^-v-v-^^-^-^""^^'^ ^ 0.08 0.075 ' ' ' ' ' ' ' 0.1 0.2 0.3 0.4 0.5 0.6 Figure 4.32: Leg accelerations for a platform motion with translation and constant acceleration. a Leg Forces of the Platform 6 4 . Si a. £i, £i & £^ a. d £i £i a ^ ^ j^ a £i t. Legl(l) - Leg2(q) Leg3(m) -- Leg4(r) V Leg5(n) - Leg6(p) V 'vvvvvvvvvvvvvvvvvvvvvvvv z 2 - - -2 .4 1 1 1 1 1 C t 0.1 0.2 0.3 0.4 0.5 time 6 Figure 4.33: Leg forces for a platform motion with translation and constant acceleration. 89 0.9 Leg Displacements of the Platform 1 LegKD — Leg2(q) Leg3(m) 0.85 ~ y Leg4(r) " V Leg5(n) / V Leg6(p) 0.8 y V - .,....■■. ^.iV- ^ : V ■ ' ♦ . s ■'" - X ■' ■ > ^ '-...i- . \ '" " X V 0.75 E 0.7 0.65 ^ ~ .- V ..■■■■ ' - ^ ^ V >7 0.6 V ^ ^ ^>■••■■■■ ^^-^-?~ll -■■■■••■■■■■■' ^^-. ■ 0.55 1 1 1 1 1 - 0.1 0.2 0.3 time 0.4 0.5 0.6 Figure 4.34: Leg displacements for a platform motion with rotation about the vertical axis, no translation, and constant angular acceleration. 1 Leg Velocities of ttie Platfomn Legl(l) ^ "" Leg2(q) Leg3(m) y^ ■ Leg4(r) y^ Leg5(n) Leg6(p) X V ^ ^ ^ ^ v> 1 0.5 / V / / ^ ^ / / . ^ "^ '' / - ^ A ,^ / V ^ - ^ ^^ y '^. /» "■ .. >-■'''' n»i 1 1 1 1 1 0.1 0.2 0.3 time 0.4 0.5 0.6 Figure 4.35: Leg velocities for a platform motion with rotation about the vertical axis, no translation, and constant angular acceleration. 90 Leg Accelerations of the Platform 8 LegKD ^ --- Leg2(q) 1 - / Leg3(m) / Leg4(r) 6 / V Leg5(n) - / Leg6(p) / /ii -^ ■ ■ - , / £1,' / / / -"/ 4 / ^ / O 7"^->^. i! 2 -? . ' - V - -A 1 1 1 1 1 0.1 0.2 0.3 time 0.4 0.5 0.6 Figure 4.36: Leg accelerations for a platform motion with rotation about the vertical axis, no translation, and constant angular acceleration. 200 150 100 50 Z Leg Forces of ttie Platform -50 -100 -150 -200 1 1 1 I y ■■ 1 V Legl(l) Leg2(q) Leg3(m) Leg4(r) Leg5(n) Leg6(p) 1 - V V V V 'V' V V V V V V - V, \ ' \ \ 1 1 1 1 0.1 oz 0.3 time 0.4 0.5 0.6 Figure 4.37: Leg forces for a platform motion with rotation about the vertical axis, no translation, and constant angular acceleration. CHAPTER 5 PATH TRACKING AND CONTROL This section discusses the control of a manipulator by developing a system model and applying the path tracking techniques developed in Chapter 4. Manipulator Model A manipulator model was developed for testing the path tracking algorithms developed and to study the system response. The model was based on a three link planar manipulator with all three joints being revolute. The link lengths were selected which defined the geometry of the manipulator as well as the motor torque requirements. It was decided to use electric motors as direct current (DC) motors lends themselves well to speed control applications. The torque requirements were calculated based on the mechanism geometry and performance desired. The mechanism parameters are (Figure 5.1): /i = 30 cm, h = 30 cm, and /i = 15 cm, with a payload of 2.5 kgs. Dynamic equations of the motor are used to determine the motor transfer function. The transfer function is then used to model the system parameters. The Figure 5.1: A 3R Planar Manipulator. 91 92 electrical equation of a motor is given as [Rel]: V = L^^ + RI,+K,co (5.1) where j^ ; ., , a^ •. ' ^ - . ' V = motor voltage '*> L = motor inductance a /^ = motor current /?= motor resistance AT^ = electrical constant, and (o = motor velocity. The magnetic field in the motor is constant and thus the current produces a proportional torque (Tg), The opposing torque of the motor (r„) is a function of the constant friction torque (T^), and the viscous friction proportional to the velocity ( Deo ), and can be written as r„ = T^+Dco. (5.3) The relationship between the torques and velocity is the following: Tg = {J„+J,)^ + DcD + T^+T, (54) where J^ and J, are the moments of inertia of the motor and the load respectively, and 7] is the load opposing torque. Equation (5.4) is the dynamic equation of the motor, and along with Equations (5.1) and (5.2), it describes the relations between the electrical and the mechanical variables. 93 When a motor is used as a component in a system, the transfer function of the motor is required which can be determined from the equations described above. Assuming T, and Tj- equal to zero, since neither effect the motor transfer function. Equations (5.1), (5.2), and (5.4) can be written in the Laplace domain as V{s) = {sL^+R)l^is) + KMs) (5.5) T,(s) = iJ„+J,)scois) + Dcois). (5 7) Representing the total moment of inertia as J , Equations (5.6) and (5.7) can be combined to derive an expression for the current which when substituted in Equation (5.5) yields V{s) = -^{sL^+R){sJ + D)cois) + K,o>(s) (5 g^ and the transfer function of the motor is V{s) K, (5-9) {sL^+R){sJ + D)+K^K/ The transfer function has two poles, which in all practical cases are negative and real. The transfer function can be written as {ST,+\){sT^+\) K, \ (5.10) JL^ (s-Pi)is-P2) where r, and Tj are the time constants and p^ , p^ are the poles of the transfer function. The time constants are related to the poles by 94 T, =-l/p, and r2=-l/p2. (5 11) Considering the case where the damping factor D is negUgibie (=0) and the inductance L is small, the transfer function becomes .- G„{s) = Kr s^LJ + sRJ + K^Kj. (5.12) and the poles are the roots of the characteristic equation s^LJ + sRJ + K^Kj. = , (5.13) Using the approximation for L^ being small, the poles are found to be — K. cK.j- , — K and P2 = RJ '' L and from Equation (5.1 1) the time constants are RJ , L^ r„ = and r, = — 2- £ r (5.14) (5.15) where r is the mechanical time constant and r^ is the electrical time constant. The relationship between the poles and the constants is valid only when the mechanical time constant is an order of magnitude larger than the electrical time constant, which is true from most DC brushless servo motors. Motor Specifications The torque requirements for the motors were developed based on the manipulator size and desired performance. Based on the requirements, DC brushless servomotors were the motors of choice; and specifications are shown in Table 5.1. Table 5.1 Manipulator specifications. Link 1 2 3 Link Length 30 cm 30 cm 15 cm Torque Requirement 4.97 N.m 1.12N.m 0.26 N.m Motor Selected Electro-Craft F4050 Electro-Craft Y 2012-1 Electro-Craft LD 2003 Torque (cont./max.) 5.2/13.6 N.m 1.4/3.8 N.m .34/1.02 N.m 95 System Model For the purpose of this analysis, the motors are modeled as a first order system and step-response modeling is carried out. The basis for this assumption is the fact that overdamped second order and higher order systems can often be approximated by a first order system with dead time [B0I88]. The step-response indicates the form of the process and the coefficients in the process model can be determined. The input output relationship (transfer fianction) of a first order system in the Laplace domain is given by yjs)^ 1 xis) Ts+\ (5-^6) where t is the time constant. Applying a unit step function \/s as input in the equation above, we get y(s) = - s rs+l and rearranging yis) = and subsequently taking the inverse Laplace transform yields t yis)^\-e'. (5.18) It should be noted from Equation (5.18) that the smaller the time constant r , the faster the system response. For implementing the system model, a discrete process model was used. The discrete transfer function is based on the use of difference equations. The backward shift operator B is used, which is defined as 96 By„ = >'„-,, B y„^ y„_^, ... , B^y„ = y„_j. (51 9) The discrete transfer function for a first order system with a time constant is given by X. K(\-e-"')B \-e-"'B ' and the discrete transfer fUnction for a delay process is given by ^ = KB'', where T - sample time T = time constant of the motor B - backward shift operator, and K= motor gain D = delay, and d = D/T = an integer. The transfer function for a first order process with a delay then becomes X. K{\-e-^")B'''' \-e-"'B and is shown in Figure 5.2. Figure 5.2: A process with delay. (5.20) (5.21) (5.22) PROCESS DELAY w W W ^ 97 Controller Development Once the process (motor) model has been developed, the design of an appropriate controller was the next step. The objective being to design a controller that will allow a response that meets the transient and steady state requirements of the closed loop system. The most widely used controller in industrial control systems, the proportional-integral- derivative (PID) controller is a combination of the three basic control actions and provides the advantages of all the three individual control actions. The equation of a controller with PID action is given by . y y{t) = K^ e{t) + ^ ]e{t)dt + K^T, ^ (5.23) where e is the error between the input and the output, K^ is the proportional gain, T. is the integral time, and Tj is the derivative time. The transfer function of a PID controller IS 2(£) ^ ^ r. 1 ^ eis) (5.24) or alternatively it can be written as — - ^.+V + ^.^ (5.25) where K^ is the integral gain and K^ is the derivative gain. In this case, when a controller is tuned, the gains are the adjustable parameters. Once the controller type was selected, controller tuning for the process was done. Controller tuning is a method of selecting the controller parameters to meet the desired performance specifications. The method employed for tuning was the Ziegler and Nichols rules for tuning PID controllers. The Ziegler and Nichols rules [Oga97] are 98 based on experimental step responses or the value of K^ that results in marginal stability when only the proportional control action is used. The second method of Ziegler and Nichols rules was used for determining the controller parameters. In this method, the integral time 7] is set to infinity and the derivative time T^ is set to zero. Using the proportional control action only, the proportional gain is increased from to a critical value K^^ where the output first exhibits sustained oscillations. The critical gain K^^ and the corresponding period P„ are experimentally determined. The critical period is the time period for one complete oscillation of the output with critical proportional gain. fciO - PID ^ 1 1 1 ^S L .005753S+1 step Input PID Centre ller T ranspo Delay rt Transfer Fen Response Figure 5.3: A closed loop system for controller tuning. The tuning system was set up using the Simulink® software by modeling each motor in a feedback loop with a controller. The setup for each motor resembled the block diagram shown in Figure 5.3. The time constant, r , for each motor was calculated using the data in Table 5.2 and was found to be Table 5.2: Parameter values for determining the mechanical time constant of motors Motor 1 (F 4050) 2 (Y 2012-1) 3 (LD 2003) R (Ohm) .54 1.3 3.0 J(kg-mO .0022 .000032 .000013 A:^(V/kRPM) 66 29 16 /^^(Nm/A) .54 .24 .15 99 r, = 0.005753 sec. 72 = 0.00063 sec. Tj = 0.00170 sec. The integral and derivative gain values were set to zero and the proportional gain was varied until the response exhibited sustained oscillations. This gain was further tweaked to within a reasonable precision. Figure 5.4(a,b) illustrates this process. Once the critical gain was obtained, the critical time period was determined, and subsequently, using the Ziegler Nichols rules, the proportional gain was obtained as K^=0.6K^^, (5 26) following which, the integral time 7] , and the derivative time T^ was calculated Z = 0.5 P 7;= 0.125 P,, which finally yielded the gains as K.=—^ and .5 P„ K,^0.\15P,^K^. Step mput RMponM «Mi PID gMn« (5.27) (5.28) (5.29) (5.30) 0.1 0.2 0.3 04 0.3 0.0 0.7 09 0.9 1 0.1 0.2 0.3 0.4 0.5 OJ 07 OJ 0.0 \ Figure 5.4: (a) Sustained oscillations at critical proportional gain (Motor 1). (b) Response to a step input with the PID controller (Motor 1). 100 It is important to note that the values obtained by this method are essentially an educated guess which provides a starting point for fine tuning the system, and hence the systems were fine tuned to obtain a smooth step response. It is, of course, also possible to add other advanced techniques such as feedforward control, disturbance rejection, adaptive control, or a combination thereof, for designing an optimal controller, but for the purpose of illustrating the path tracking in this case, PID control was deemed sufficient. Controller Implementation A discrete PID controller was implemented to control the process described in Equation (5.22). The controller is of the form m. where = w„_, + ^0 er„ + k, er„_, + k^ er„_^ (5 j^) m„ = controller output er = (command - actual) error: controller input n-i= values at i time-steps back, and r^, A:, , ^2 , are constants computed as = K+KJ + ^ p > Y (5.32) ' " T (5.33) 2 J, (5.34) where T is the sample period. Once the process transfer fiinction and the controller transfer fiinction were determined, the control loop was setup as shown in Figure 5.5. The discrete controller 101 algorithm was implemented in MATLAB and the results were tested to correspond to the continuous time controller used in Simulink to model the step response. Once the results were verified, the control algorithm was amalgamated with the path-tracking algorithm for a 3R Manipulator. >^ ^\ PID CONTROLLER PROCESS yn """.^ ""% DELAY W ►(* > \ / p i Figure 5.5: Closed loop PID controller for a process with delay. The discrete path-tracking controller uses the following equation, based on Equation (5.22), to generate its output that is dependent on the previous value of the output and the control effort generated by the controller. Using Equation (5.22) and cross multiplying, we obtain , , ^ y„ = e-"^By„^Kil-e-''^)B''^\. Finally, applying the backward shift operator (5.35) (5.36) (5.37) yn = e-"'y„_,+K{\-e-"')x„^,,,. Writing the equation in terms of control variables we obtain. (5.38) ^a(n) = e ^ «„(„.„ + /: 1 1 - e ^ I m„.,_. (5.39) where 102 0)^ = system output (actual angular velocity) at time step n ®a(n-i) - actual angular velocity at previous time step fn„_i_d = control effort at previous time step or earlier depending on the delay. The controller was implemented for a 3R-manipulator path tracking using the methodology developed in Chapter 4. The flow chart for the algorithm is shown in Figure 5.6a and a block diagram of the process is shown in Figure 5.6b. The algorithm essentially represents an open loop tracking control of the manipulator. The controller loop operates at a higher frequency than the tracker command frequency. The option to adjust the controller loop frequency is a feature in the program. Another variable is the delay time D (d in Equation (5.39)), which can be fluctuated to study the behavior and response of the system and evaluate its performance. Initially the program (main54cPlDlc.m, Appendix B) was tested with a delay of 3 milliseconds (ms). To arrive at a reasonable estimate for a delay time, the following idea was used: a first order system, in response to a step input, reaches 99.3% of the final value in 5 time constants. Using this number and the motor with the highest time constant, and the assumption that the delay is approximately .5-5% of 5 time constants, the delay was found to be 1.5ms. For a conservative estimate, the delay used was doubled to account for any other delays in the computer control of the manipulator. Figures 5.7 and 5.8 illustrate the actual and command angular velocities for each motor and the errors between the two. This test was performed with a delay of 3ms. 103 PATH TRACKER COMMAND VELOCITIES W„ Controller calculates error (actual- command) er„ Calculates control effort: Equation (5.31 ) m„ Outputs Effort to Process/ Updates Variables Controller Calculate Output of the system using Equation 5.35 W. /Generate Output A A Update time J W„ PATH TRACKER \^\ RID CONTROLLER '"% PROCESS ^a +> DELAY -\ ) ^ iL Figure 5.6: (a) Flowchart illustrating the algorithm for path tracking and control. (b) Block diagram of the process. 104 Actual and Command w1 0.2 -0.4 -0.6 ( 0.5 1 -0.5 x. W1. -^-^ / 12 3 4 Actual and Command w2 *2, V 12 3 4 Actual and Command w3 0.5 1 -0.5 ■ / __ w3. ^^y 2 3 time 0.02 0.01 -0.01 5 ' -5 -10 -15 I 0.15 .xlO ftt 0.05 -0.05 En^or In w1 2 3 Error In w2 2 3 Error in w3 2 3 time Figure 5.7: Actual and command angular velocities and errors 2. Actual and Command w1 0.19 0.18 \ \ _W1^ — w1. P — — ^~~~^ 0.17 '"'"'^''-^-^ — , 0.16 ■ xlO Error in w1 0.02 0.03 0.04 0.05 Actual and Command w2 0.1 ,0.08 I I 0.06 _.JH?^"; ^-yrrr^' — ^c -W2. 1 ■ 0.01 0.02 0.03 0.04 0.05 0.06 Actual and Command w3 -0.24 I -0.26 -0.28 ' — w3^ - w3. ^^ 0.05 0.1 0.15 0.2 0.25 time 15 10 5 0.5 1.5 xlO^ En-or in w2 ' -5 • 10 0.5 1 time 1.5 Figure 5.8: Magnified view of the command velocities and errors. 0.5 1 En^or in w3 1.5 2 0.04 ' ■ 0.02 • -0.02 105 ■"■. t\ To determine the accuracy of the end effector of the manipulator, the errors in the end effector position were calculated. The error was determined by obtaining the difference between the command and actual positions (using the forward analysis) in both the X and y coordinates. Figure 5.9 illustrates these errors. Furthermore, the statistical error analysis yielded the following results: Table 5.3: Errors in the end effector position. Error Mean (cm.) Standard Deviation (cm.) x-coordinate of the end effector 0.03786 0.05765 y-coordiante of the end effector 0.02666 0.06854 .X10 Error in x coordinate of end effector 1.5 2 2.5 3 3.5 time Error in y coordinate of end effector 4.5 1.5 2 2.5 3 3.5 4 4.5 time Figure 5.9: Errors in the end effector position. On subsequent tests, the delay was increased step by step until the system became unstable and exhibited oscillatory response. The onset of oscillation was at 50ms and at 106 higher delay times the system did not converge. Figure 5.10 shows the command and actual velocities at the 50ms-delay level. The onset of oscillation is apparent. 0.3 ^0.2 0.1 0.1 0.05 ^ Actual and Command w1 I 'I .'if ' iiiiiiiiiU II lip W1^ Wl 0.2 0.4 Actual and Command w2 'i-/-^ _w2^ - - w2. '/ • 0.05 0.1 Actual and Command w3 0.22 ■ — w3^ 0.24 A _-w33 0.26 1 \ « /I / \ 1 — LJ ( \ • 0.28 0.05 0.1 time 0.15 Error in w1 0.5 Error in w3 1 0.04 0.02 ^1 \k^ r 0.02 v 0.04 , . 0.1 0.2 tiine 0.3 0.4 Figure 5.10: Actual and command angular velocities and errors with the Delay set at 50 milliseconds. It is possible to retune the system to where it can handle the higher delay. The penalty of doing so is the higher rise time or sluggish response of the system. Experimentation was done for this purpose and the results showed that for systems tuned to a 100ms delay, the rise time was approximately 2.3 times that of a system tuned for a 10ms delay. A sample response test performed on motor 2 is shown in Figure 5.10a and Figure 5.10b. 107 Response to step Input: 10ms delay 0.8 0.4 0.2- ■ :■;;? 0.1 0.2 0.3 0.4 0.5 0.6 0.7 time Figure 5.10(a): Step response for a system with 10ms delay. Response to step Input: 100ms delay Figure 5.10(b): Step response for a system with 100ms delay. The results of the simulation indicate that the controller will perform well within a range of delay times. If the delay times exceeds a threshold, the controller can be retuned although this would result in a sluggish response from the manipulator. It is anticipated that this method will produce similar results when applied to an experimental system. CHAPTER 6 CONCLUSIONS AND FUTURE WORK Conclusions The Theory of Screws provides an elegant and useful method for analysis of mechanisms. In this research, the concepts of Screw Theory have been employed to develop path tracking methodologies for autonomous robots, serial robots, and parallel manipulators. The method for autonomous robots and serial manipulators involves generating a command screw based on the constraints specified. The constraints can be at the position or the velocity level. For position level constraints, a command screw is generated based on the velocity screw. For velocity level constraints, a command screw is generated based on the combined effects of the velocity and acceleration screw. For autonomous robots, the accuracy of the generated command screw is verified by choosing a circular path and plotting the poles of the command screw. For serial robots, commands are generated using the reverse analysis, and the generated commands are compared to either the desired velocity state or the desired velocity and acceleration state depending on the constraints. Tracking for parallel manipulators is accomplished by introducing a polynomial that satisfies conditions for each time step. The conditions were assumed to be the position, velocity, and acceleration at each time step. A fifth order polynomial is selected that has six constants which satisfy the position, velocity, and acceleration conditions at 108 109 the beginning and end of each time step. The methodology involves the reverse analysis to generate the displacements, velocities, and accelerations, of the platform cormectors based on the desired motion. Using this information the constants of the polynomial are determined which is used as the command for position, velocity, and acceleration. The analysis for the parallel platforms was further extended to include the inverse dynamics to determine the forces in the legs. This was considered a beneficial tool for implementing a tracking and control strategy for a parallel manipulator as the output is an indicator of the hardware performance requirements of the system. Path tracking with control was implemented on a 3R serial manipulator. This was done to study the performance of the path tracking algorithm with the dynamics of the system. A serial manipulator was modeled, which used electrical motors for actuation. The motors were modeled as a first order system with a delay. A PID controller was developed for motor control and was tuned for each motor. The results indicated that the controller followed the desired tracker commands within a band for the delay. In summary, a methodology has been established that employs the Theory of Screws. It is by no means comprehensive but lays the foundation for Screw Theory based path tracking and control for general categories of robots and manipulators. Future Work The method developed for the autonomous robot does not consider vehicular constraints. The method can be extended to address constraints that are particular to an autonomous vehicle, such as vehicle with front wheel steering or vehicles with all wheel steering. 110 The methods do not consider constraints on velocities or accelerations. These limits can be incorporated into the algorithm when a specific application is known and the system limitations are known. The algorithm can then generate commands based on the best achievable value. Another interesting prospect is the application of the parallel platform methodology to the spatial jack being developed at CIMAR. It is expected that this method will provide the basis for the tracking control of the platform. It is also expected that by being implemented on the manipulator, the algorithm will evolve to a better and more useful tool. APPENDIX A INITIAL PATH TRACKING APPROACHES f *; 112 Initial path tracking approaches are presented here, the results of which led to the development of path tracking techniques discussed in the main text. 113 Considering the Pose and Velocity Information ' In this case, the velocity state of the rigid body B is specified, in addition to the pose (position and orientation) information that is available. This is illustrated in Figure A.I. PATH . Desired pose at time (t,,) Desired pose at time (to+dt) Yc Oc(to) Xc Current pose at time (t.) ^o**^ \ X Figure 4. 1 : A rigid body following a path with a prescribed pose and velocity. In the most general case, the problem can be stated as: given the current pose of a rigid body B at time to, together with its velocity state at the same time to, and the desired timed path of the body, together with the desired time dependent specifications of the velocity state, develop a tracking technique so that at time to+At, the desired and the current poses and velocity states coincide, respectively. The problem of computing rotations about arbitrary axes is quite complicated. At the outset, it was attempted to solve this problem with a simplified analysis involving rotations around a fixed axis combined with translations. This assumption is justified as to this point, as the initial analysis cases are two-dimensional. Once a technique is devised, it will be attempted to devise methods to include arbitrary spatial rotations. 114 Rotation around a fixed axis In this case, the problem becomes as shown in Figure A.2. Given the current position 9c at time to and the 0c(to),COc(to) current velocity coe at time to, it is required that time to+At, both the desired and current position and the desired and current velocity state coincide. Assuming constant angular acceleration a, and integrating the angular speed equation co-dOldt, can be written in difference form as: 0)^ {Iq + At) = 0)^ (to ) + aAt and integrating again, e^ {t, + At) = G^ (t, ) + 0)^ (t, )At + X aA/ ' ed(to),o)ci(to) ed(to+At),cOd(to+At) Figure A.2: Rotation about a fixed axis. (A.1) (A.2) In general, it will be impossible to simultaneously solve (A.l) and (A.2) with only one variable, a, available. To overcome this problem, the following concept is introduced. Two acceleration states will be used, i.e., ai and a2, such that a(t) = \a, to < t < (/o+f) (A.3) l«2 (^+f) < i < (^o+AOj and is illustrated in Figure A.3. Two equations with two unknowns are now solvable. Integrating the equations as done previously, with a different time interval. 115 6^c(^o+f) = ^cOo) + «iY (A.4) G),Oo+^) = <»c(^0+f) + «2 A/ substituting, the former into the latter equation results in: 6>c(^0+^) = ^c(^o) + — ! ^A/ and finally, 0),{t,) + ^'\^^ M^O},{t^+^.) (A.5) Similarly, for the orientation we have: i ^ a(t) a to i ai ' a(t) t a2 ^r . to t Figure A.3: Acceleration profile. A/ 1 ^c(^o+f)= ^c(^o) + ^c(fo)^ + T« ^A/V 2 2 A/ 1 TA/^ ^,Oo+A/)= ^,(/o+f) + «c(^o+f)Y+2" V ^ y (A.6) and as done previously, substituting the former into the latter: A/ 1 TA/^ ^c(^o+^)= ^cOo) + ^cOo)y+2« Ar 1 Ta/^ V ^ y + «c(^o+f)y+2« \ ^ J substituting the value for fi;^(/o +-y) from Equation A.5 ^c(^o+^)= ^c(^o) + «cOo)y+2«i A/ V ^ y A/ 1 — + — a. 2 2 ^ ^A/^' 2; ^,(/o+A/)= ^,Oo) + «cOo)Af + -«i ^A/^ + —a. \^ J At 116 finally results in 3 ^4(Y,i„r4 2 'I 2 J 2 H 2 J (A.7) Equations (A.5) and (A.7) lead to the following system of linear equations: 2 a, + oTj = 3«, + a2 At 8 A^ (A.8) Since the coefficient matrix of the above system is non-singular, 1 1 3 1 1-3 = -2^0, the system always has a solution. The solution is unique assuming the time interval is halved; solutions can be obtained for other fractions of the time interval. Translation in Space The problem statement in this case is, given the current position vector Fc and velocity vector Vc at time to, as well as the desired position vector Fd and the velocity vector Vd find the motion states so that Z=X^ i i. Vd(U^ r V Vd(to+At) rd(to) ^-^^'"^^^i" ^ '♦., rd(to+At) ^ / r.(t„) Vc(to) y=x^ x=x' at time to+At, both the desired and current conditions coincide. This is shown in Figure A.4. Since the solution is the same for each of the translation Figure A.4: Translation in space, components, instead of employing the Cartesian x, y, z axes, the axes x', x^, x^ will be 117 used. Furthermore, the problem is totally analogous to the rotation around the fixed axis problem, and thus the linear acceleration specification is given as: a'(0 = a, L«2 to<t<{to+T) 1 And following the same procedure as in the prior section in the rotation case, the following equations are obtained: y'M + ^^^^(-y'Ato + ^) 3 ^At^' \^ J AM 2 H 2 (A.9) (A. 10) Thus, for any /= 1,2,3, the linear system becomes, a;+fl2 = — k(fo+^)-<(^o)] 3a,' +a\ = Af -kit,+A,)-r:{t,)-vUtoM (A. 11) Amalgam of the Translation and Rotation Analyses The two previous analyses can be combined to provide a solution for the path tracking problem of a serial, non-redundant, planar manipulator. Combining the results of these analyses, the velocity and reduced acceleration states for the end-effector of the planar robot can be obtained. Considering the poses and the velocity states of the end- effector, the velocity and acceleration specifications' for the end-effector at time t=to are: The index in the formulas goes from to 3 because a 3 -link manipulator is considered. 118 'V\(o) = Ki^o) = 'A,\t,) = Or 3, 0,-t 3 O.-r 3, 5o, (^o)-X(^o)x>oOo) (A.12) (A.13) where d^ (t^) and Oo, (/g) are obtained from the analyses done in previous sections. The joint velocities and accelerations can be obtained from the following set of equations [Ric99]: : U \*,^ '' / '* ' * I 'V'ito) = oMt,)'$\t,)+ ,co,it,)'$\t,)+ ,(0,it,)'$'it,) 'A,\t,) = \ d)M°$\t,)+ ,cb,it,)'$'{t,)+ ,(b,{t,)'$\t,) + coM'$\t,) ,co,(t,)'$\t,)+ ,co,{t,)'$'{t,)\ + (A. 14) (A.15) where the quantities in [ ] brackets represent the Lie product of two screws. For time t=(/o + y) , the velocity and reduced acceleration expressions become: 0t73 V\t,+f) = At Or. 3 0- 3, V'o Oo)+ ^01 (^o) (A. 16) 'A,\t,+f) = Vc^o) «^/(/o)+<'aA/o)f Vo^(/oKX^(^o)f (A. 17) The new joint velocities and accelerations are obtained from Equation (A. 16) and Equation (A. 17) evaluated at t=(/o +-f-) as follows: 119 0t73 V\L+f) ''A,\t,+f) = \ + 2^3('o+f)'$'(^o+f) + (A.18) (A.19) It must be pointed out that during the motion from t=to to t=to+At, there will be changes in the location of the screws which would yield a result that would not be exact, and as a consequence of which, there would be a small error between the desired and required poses and velocity states. However, it can be hypothesized that with the selection of a small interval At, the effect of this error can be minimized. Path Tracking Assuming a Linearly Varying Acceleration Profile Consider the initial and final acceleration after a time step At : ao=a(Oat/o . , Oj^ = a{t) at tg+At . Now assuming that the variation is linear, it can be written that ait) = flo + At fl(/)«ao+— (a.-flo) At (A.20) (A.21) then For t=0, u{t) - Vq , thus ^iO = aot + —{af-a,)+q. u(t)^u,+a,t + —(af-ao). (A.22) (A.23) Integrating again, 120 aJ^ t^ sit) = Oot + ^ + -—{a J. -a,)+ Q . 2 6A; Again, for t=0, 5(0 = Sq , thus (A.24) aJ t The values of a . and a^ must satisfy the conditions (A.25) therefore and s{i) = Sj- at /() + At and u{t) = Uj^ at t^+At Or=OQ+ a^At + TT-(a/ - ^0 ) 2A/ aAt^ At^ ^(,At Ar / \ s{t) = s,+v,At + ^:^ + —[a^-a,). Rearranging the equations, (A.26) (A.27) Vj -Uq- At V 2 2, f Sf -Sq= UoA/ + Ar ^2 6 6^ (A.28) (A.29) or Vf-V^ = At [a.+a^) f Sj- -Sq- UqA/ = At' ,3 6, (A.30) (A.31) and finally 121 2(Of-Oo) At 6(sj- -So- UqA/) ^ =(a/+«o) ■ = 2ar,+ar. At' Solving for a^ and a^ 2(vjAt-3Sf + 3^0 + lu^At) Ae 2\^AtVf - 35^ + 3^0 + v^At) r' ;, * ^tjf. (A.32) ^^2 -»o+«/- (A.33) (A.34) «/ = X? • (^-35) APPENDIX B SIMULATION SOURCE CODE J! 123 -'■ >■ ; " ; ■, . ,■ , 5.> K Source code for the simulations using MATLAB 124 autonomous. n % Implementing path tracking on an Autonomous robot % or Vehicle. % Using the Position and Orientation information ONLY % (No velocity information) % % Previous: particle. m % Next: autonomousl .m D2R = pi/180.; % Degrees to Radians conversion factor r = 0.5; t0 = 0.0; . ' Cx=1.0; . Cy=0.0; tf=6.20; % 6.5 delta = 0.05 ; ■ »■•■, ' % Initializing all the vector arrays ta = [] ; xOda= [] ; iv.,' yOda= [] ; ■: ro^a = []; • v^ ,.■,; i U-> f, rOya = [] ; ' ' ' r01c=[1.51; -0.05; 0] ; thetac=1.7; % The Loop starts over here for the duration tf while tO<tf rd = [Cx + r*cos (tO+delta) ; Cy+r*sin(tO+delta) ; 0]; they=atan2 (rd(2, 1) -Cy, rd(l, 1) -Cx) ; if they<0 thetad=they + 2.0*pi + pi/2; else thetad = they+pi/2; end; : % Calculations for computing the required velocities % Calculating the angular velocities and accelerations V0c= [ 0; 0; (thetad-thetac) /delta; (rd (1 , 1) -rOlc (1 , 1) ) /delta; (rd (2 , 1) r01c(2,l) )/delta;0] ; w=[ V0c(3,l)] ; V0=[V0c(4,l) V0c(5,l) 0]; r0=r01c+(Cross(w,V0)/V0c(3,l)*2) ' ; %The location of the velocity pole is rOx = rO(l,l) ; rOy = r0(2,l); ta = [ ta [ to + delta ] ] ; rOxa = [ rOxa [ rOx ] ] ; rOya = [ rOya [ rOy ] ] ; xOda = [ xOda [ rd(l,l) ]]; 125 yOda = [ yOda [ rd(2,l) ] ] ; r01c=rd; thetac=thetad; to = to + delta; end . '■ figure; ^- ' ■• plot (rOxa,rOya, 'o' ) , title (' Path Tracking (pose) : Screw Locations llQll 1 ) hold on plot (xOda,yOda, 'r+' ) ,. / %axis( [-2 2-2 2]) legend ( 'pole locations' , 'path' ) ; axis equal; zoom on; - grid on; hold off 126 autonomous 1 .m % MAIN PROGRAM % Implementing path tracking on an Autonomous robot % or Vehicle. % Using the Position and Orientation information ONLY % (No velocity information) % % Previous: autonomous. m % (the only new thing is different starting point ) % Next: autonomous2 .m D2R = pi/180.; % Degrees to Radians conversion factor r = 0.5; • , . to = 0.0; '■ ♦ Cx=1.0; Cy=0.5; tf=1.9*pi; delta = 0.05; , % Initializing all the vector arrays ta = [] xOda=[] yOda=[] rOxa = [] ; rOya = [] ; ' thetada = [] ; -■ , . r01c= [1.50; 0.5; 0] ; thetac=pi/2; % The Loop starts over here for the duration tf while t0<tf rd = [Cx+r*cos(tO+delta) ; Cy+r*sin(tO+delta) ; 0]; they=atan2 (rd(2,l) -Cy, rd(l, 1) -Cx) ; if they<0 thetad=they + 2.0*pi + pi/2; else ' thetad = they+pi/2; end; . .,, „. ... I % Calculations for computing the required velocities % Calculating the angular velocities V0c=[ 0; 0; (thetad-thetac) /delta; (rd (1 , 1) -rOlc (1 , 1) ) /delta; (rd (2 , 1) r01c(2,l) )/delta;0] ; ...... ... w=[ V0c(3,l) ] ; V0=[V0c(4,l) V0c(5,l) 0]; rO= r01c+ (cross (w,V0)/V0c (3,1) *2) ' ; %The location of the velocity pole is '. " rOx = rO (1 , 1) ; rOy = r0(2,l) ; 127 ta = [ ta [ to + delta ] ] ; rOxa = [ rOxa [ rOx ] ] ; rOya = [ rOya [ rOy ] ] ; xOda = [ xOda [ rd(l,l) ]] ; yOda = [ yOda [ rd(2,l) ] ] ; thetada = [thetada [thetad] ] ; r01c=rd; thetac=thetad; to = to + delta; end figure; plot (rOxa,rOya, 'o' ) , title (' Path tracking (pose) : Screw Locations "o" ' ) ... hold on plot (xOda , yOda , ' r+ ' ) legend ('pole location' , 'path' ) ; axis equal; .. %axis( [-2 2-2 2]) ■ grid on; zoom on • , hold off •■3t . t 128 % autonomous 2 a. m % Implementing Path Tracking on an Autonomous Robot/Vehicle % specifying the initial position and the final % position and velocity states % Circular Path / Draws the orientation as a box % % Previous: autonomous. m D2R = pi/180.; % Degrees to Radians conversion factor r = 0.5; • • to = 0.0; Cx=1.0; Cy=0.5; tf=6.1 ; % original value 6.3 . ^. ,_^. .. ^ delta = 0.1; ,':'i:f*c '..-•/■' '''I' % Initializing all the vector arrays ta = [] ; - xOda= [] ; yOda=[] ; : ■ ' ■" ■■'•■;./ '■ J- ./ rOxa = [] ; rOya = [] ; thetada = [] ; angaa= [] ; axa= [] ; aya= [] ; omegaa= [] ; vxa= [] ; vya= [] ; r01c=[1.54; .45; 0] ; thetac=pi/2 + . 002 ; % The Loop starts over here for the duration tf while tO<tf % path is specified as a circle at an offset (Cx, Cy) from the Origin rd = [Cx+r*cos(tO+delta) ; Cy+r*sin(tO+delta) ; 0]; they=atan2(rd(2,l) -Cy, rd(l, 1) -Cx) ; if they<0 thetad=they + 2.0*pi + pi/2; else thetad = they+pi/2; end ; vd = [-r*sin(tO+delta) ;r*cos (tO+delta) ;0] ; omegad= 1 ; % Calculations for computing the required initial velocities % Calculating the angular velocities and accelerations V0c= [ 0; 0; 2* ( (thetad-thetac) /delta) -omegad; 2* ( (rd (1 , 1) - r01c(l,l) ) /delta) -vd(l,l) ;2* ( (rd(2, 1) -rOlc (2, 1) ) /delta) -vd(2,l) ;0] ; 129 As=[0; 0; (2/delta) * (omegad- ( (1* (thetad-thetac) ) /delta) ) ; (2/delta) * (vd(l,l) - ( (1* (rd(l,l) -r01c(l, 1) ) ) /delta) ) ; (2/delta)*(vd(2,l)-( (l*(rd(2,l)-r01c(2,l)))/delta)) ; 0] w= [ V0c(3,l) ] ; V0=[V0c(4,l) V0c(5,l) 0] ; rO= r01c+ (cross (w,VO)/VOc( 3, 1)^2) ' ; %The location of the initial velocity pole is rOx = rO(l,l) ; rOy = r0(2,l) ; ta = [ ta [ to + delta ] ] ; " ■ rOxa = [ rOxa [ rOx ] ] ; rOya = [ rOya [ rOy ] ] ; xOda = [ xOda [ rd(l,l) ]] ; yOda = [ yOda [ rd(2,l) ]] ; thetada = [thetada [thetad] ] ; angaa = [angaa [As (3,1)]]; «--•;, ■• axa= [ axa [As (4,1)]]; " "^' ' <"' aya= [ aya [As (5,1)]]; omegaa= [omegaa [V0c(3,l)]]; ] t vxa= [vxa [V0c(4,l)]]; vya=[vya [V0c(5,l)]]; . , .^; , ^ , «, ■:: r01c=rd; thetac=thetad; to = to + delta; end figure; plot (rOxa,rOya, 'o' ) , title ('Path tracking (Pose, Velocity) : Screw Locations "o" ' ) hold on for i = 1: length (xOda) DrawBox( [xOda(i) ; yOda(i)], thetada (i) , 0.02, 0.01); end plot (xOda , yOda , ' g ' ) ; legend ('pole locations'); %axis( [-2 2-2 2]) axis equal; zoom on; hold off; 130 % autonomous 3 .n % Autonomous Path Tracking % specifying the initial position and the final % position and velocity states: Initial velocity is NOT specified % Implemented with a DrawBox function to show the change in % orientation as the vehicle traverses the desired path (Hyperbolic path) % Previous: autonomous2 .m D2R = pi/180.; % Degrees to Radians conversion factor r = O-S.- tO = -0.5; , . . ■ . .,■ Cx=1.0; , V Cy=0.5; ■ - tf = 0.5; , , .. ■■■■:^. delta = 0.05; , ■-•.,''. % Initializing all the vector arrays ta = [] xOda= [] yOda=[] rOxa = [] ; "^ rOya = [] ; thetada = [] ; angaa= [] ; axa= [] ; aya= [] ; omegaa= [] ; vxa= [] ; VYa= [] ; r01c=[4; 8; 0] ; , thetac=0.002; % The Loop starts over here for the duration tf while tO<tf % defining the trjectory of the desired motion % rd = [Cx+r*cos(tO+delta) ; Cy+r*sin(tO+delta) ; 0]; % circle % rd = [ 6 ; 5* (tO+delta) ; ]; % constant x, linear vel . y rd = [ tO+delta ; 4* (tO+delta) *2 ; ]; % parabola thetad = 5 . 0* (tO+delta) ; vd = [1 ; 8*{t0+delta) ; 0] ; ' ' .. omegad=5.0; •■;,... .-. . % Calculations for computing the required initial velocities % Calculating the angular velocities and accelerations VOc=[ 0; 0; 2* ( (thetad-thetac) /delta) -omegad; 2* ( (rd (1 , 1) - r01c(l,l) ) /delta) -vd(l,l) ;2*( (rd(2,l) -r01c(2,l) ) /delta) -vd(2,l) ;0] ; As=[0; 0; 2* (thetad-thetac) /delta*2 ; 2* (rd (1 , 1) - r01c(l,l) )/delta*2;2*(rd(2,l) -r01c(2,l) )/delta^2; 0] ; 131 w=[ V0c(3,l)] ; V0=[V0c(4,l) V0c(5,l) 0] ; rO= r01c+(cross(w,V0)/V0c(3,l)"2) ' ; ;\ %The location of the initial velocity pole is rOx = rO{l,l) ; rOy = r0(2,l) ; ta = [ ta [ to + delta ] ] ; rOxa = [ rOxa [ rOx ] ] ; rOya = [ rOya [ rOy ] ] ; xOda = [ xOda [ rd(l,l) ] ] ; yOda = [ yOda [ rd(2,l) ]]; thetada = [thetada [thetad] ] ; angaa = [angaa [As (3,1)]], • axa= [ axa [As (4,1)]], • aya= [ aya [As (5,1)]]; . omegaa= [omegaa [VOc (3,1)]]; vxa= [vxa [VOc (4,1)] ] ; vya=[vya [VOc (5,1)]] ; r01c=rd; thetac=thetad; to = to + delta; end figure; plot (rOxa,rOya, 'o' ) , title (' Path Tracking (Pose, Velocity) : Screw locations "o" ' ) hold on plot (xOda,yOda, 'c' ) legend ('pole locations ', 'path' ) ; for i = 1 : length (xOda) DrawBox( [xOda(i) ; yOda(i)], thetada (i) , .05, 0.025); end axis( [-2 2-0.5 3.5]) zoom on ; , hold off 132 % inain4.in % An example for a 3R planar serial chain to study % the proposed path tracking methodology % using initial position and final position data % Program requires the following functions: % / Screw / Draw3R % Previous: main3.m % Next : ? ? % Last Update: June 10, 2001 D2R = pi/180.; % Degrees to Radians conversion factor a_= [ 1 ; 1 ; 1] ; % Constant data, link lengths % r is a constant required by the end-effector path, r = 0.2; % The initial and final time, and the time step are given below to = 0.0; tf=6.34; delta =0.1; %Current Position Data re = [2; 1 ; 0] ; thetac = 0; [setlc, set2c, flagc] = reverse_position_3R{a_, re, thetac); if flagc == 1 error ('The initial position is outside the manipulator workspace, there is no point in completing the problem') else % Initializing all the vector arrays ta = [] ; rOxa = [] ; rOya =[] ; . ,,^^, ,, ,, .,, ,. . ."; .- thetaa=[]; J I '^J "'"O > '.M -i-.-r^ . .: «. ^ thetala=[] theta2a= [] theta3a= [] w01a= [] wl2a= [] w23a=[] ew01a= [] ewl2a= [] ew23a= [] %w01f=[] % The Loop starts over here for the duration tf while tO<tf %The desired motion and velocity of the end-effector is given below rd = [0.5+r*cos (tO+delta) ; r*sin(tO+delta) ; 0]; thetad = 0; Vd= [0;0;0;-r*sin(tO+delta) ;r*cos (tO+delta) ; 0] ; % The following statement compute the reverse analysis of the manipulator % for the required position at time tO+delta. [setl, set2, flag] = reverse_position_3R (a_, rd, thetad); if flag == 1 133 error ('The required position is outside the manipulator workspace ' ) else [Jd, waste] = Screw (a_, set 1) ; % wd are the join angular velocities computed from the desired % velocity state wd=Jd\Vd; % w are the join angular velocities computed from our tracking % algorithm w= (setl-setlc) /delta; % Calculating Errors in Position, Orientation, % Linear and Angular velocity ta = [ ta [ to + delta ] ] ; rOxa = [ rOxa [ rd(l,l) ]] ; rOya = [ rOya [ rd(2,l) ] ] ; thetaa = [thetaa [thetad] ] ; -■■ ■ : thetala = [thetala [setl{l,l)]]; theta2a = [theta2a [setl(2,l)]]; thetaSa = [theta3a [setl(3,l)]]; w01a= [wOla [ w(l,l)] ] ; ^ wl2a=[wl2a [ w(2,l)]]; w23a= [w23a [ w(3,l) ] ] ; ew01a=[ew01a [w(l, 1) -wd(l, 1) ] ] ; ewl2a= [ewl2a [w(2, 1) -wd(2, 1) ] ] ; ew23a=[ew23a [w(3, 1) -wd(3, 1) ] ] ; % Updating for the next iteration to = to + delta; • ' setlc=setl; end ■ . • ■■ end % some plotting figure; plot (rOxa,rOya) , title ('Tracking of 3R manip. using inv. analysis (Pose) ') ; Draw3R(a_, thetala, theta2a, theta3a) ; % plot(ta, wOla, ta, wl2a, ta, w23a) , title ( 'Joint Angular Velocities ' ) ; figure; plot(ta, wOla, ta, wOla+ewOla, ta, wl2a, ta, wl2a+ewl2a, ta, w23a, ta, w23a+ew23a) , title ( 'Actual and Desired Angular Velocities'); legend ( 'omegaOl ' , 'omega01_d', 'omegal2', 'omegal2_d', 'omega23', 'omega23_d ' ' '■ xlabel ( ' time ' ) ; figure; plot(ta, ewOla, '-', ta, ewl2a, '--', ta, ew23a, ':'), title ( 'Errors in Joint Angular velocities'); legend ( 'error wOl', 'error wl2 ' , 'error w23 ' ) ; xlabel ( ' time' ) ; end 134 % inain54.ni % An example for a 3R planar serial chain to study % the proposed path tracking methodology % using initial position and final position data % Program requires the following functions: % / Screw / Draw3R % Previous: main4.ra % Updated: June 15, 2001 ^ . D2R = pi/180.; % Degrees to Radians conversion factor a_= [ 1 ; 1 ; 1] ; % Constant data, link lengths % r is a constant required by the end-effector path, r = 0.2; % The initial and final time, and the time step are given below to = 0.0; tf=6.34; delta = 0.1 ; %Current Position Data ' ' ' re = [0.72; 0.1 ; 0] ; ^. , thetac = 0; [setlc, set2c, flagc] = reverse_position_3R(a_, re, thetac); if flagc == 1 error ('The initial position is outside the manipulator workspace, there is no point in completing the problem') else % Initializing all the vector arrays ,, ta = [] ; rOxa = [] ; rOya = [] ; ' ■ _■ thetaa= [] ; thetala=[]; theta2a=[]; theta3a= [] ; w01a=[]; wl2a=[]; w23a=[]; alpha01a=[]; alphal2a= [] ; alpha23a= [] ; ew01a=[]; ewl2a= [] ; ew23a= [] ; . -v eal01a=[]; eall2a=[]; eal23a= [] ; •' % The Loop starts over here for the duration tf while to<tf ... -, ;,;.:" %The desired motion and velocity of the end-effector is given below rd = [0.5+r*cos( (tO+delta) ) ; r*sin( (tO+delta) ) ; 0]; thetad = 0.0; Vd=[0;0;0;-r*sin(tO+delta) ;r*cos(tO+delta) ; 0]; As= [0;0;0;-r*cos(tO+delta) ;-r*sin{tO+delta) ; 0]; wO=Vd (1:3,1) ; vO=Vd(4:6,l) ; *" CV=Cross (wO ' ,vO' ) ' ; Ard=[As(l,l); As(2,l); As (3 , 1) ; As (4 , 1) -CV (1 , 1) ; As (5 , 1) CV(2,1) ;AS(6,1)-CV(3,1)] ; 135 % The following statement compute the reverse analysis of the manipulator % for the required position at time tO+delta. [setl, set2, flag] = reverse_position_3R(a_, rd, thetad) ; if flag == 1 error ('The required position is outside the manipulator workspace ' ) else [Jd, waste] = Screw {a_, setl) ; % wd are the join angular velocities computed from the desired % velocity state wd=Jd\Vd; Jwd=Jd*diag(wd) ; alphad=Jd\ (Ard-Paccst ( Jwd, 3,1)); % w are the join angular velocities and accelerations computed from our % tracking algorithm w=2* (setl-setlc) /delta-wd; alpha=2* (setl-setlc)/delta*2; % Calculating Errors in Position, Orientation, ''■, . % Linear and Angular velocity ta = [ ta [ to + delta ] ] ; rOxa = [ rOxa [ rd(l,l) ] ] ; rOya = [ rOya [ rd(2,l) ] ] ; thetaa = [thetaa [thetad] ] ; thetala = [thetala [setl(l,l)]]; theta2a = [theta2a [setl(2, 1) ] ] ; thetaBa = [theta3a [setl(3 , 1) ] ] ; w01a= [wOla [ w(l,l) ] ] ; wl2a= [wl2a [ w(2,l)] ] ; w23a=[w23a [ w(3,l)] ] ; ew01a=[ew01a [w(l, 1) -wd(l, 1) ] ] ; ewl2a= [ewl2a [w(2, 1) -wd(2, 1) ] ] ; ew23a=[ew23a [w(3, 1) -wd(3, 1) ] ] ; alpha01a= [alphaOla [alpha(l,l)]] alphal2a= [alphal2a [alpha(2,l)]] alpha23a= [alpha23a [alpha (3, 1) ] ] eal01a= [ealOla [alpha (1, 1) -alphad(l, eall2a= [eall2a [alpha(2, 1) -alphad(2, eal23a= [eal23a [alpha (3 , 1) -alphad (3 , 1)]] ; 1)]] ; 1)]]; % Updating for the next iteration to = to + delta; setlc=setl; end end • 7^ % some plotting figure; plot (rOxa, rOya) , title ('Tracking of 3R manip. using inv. analysis (pose, vel. ) ' ) ; legend ( 'desired path'); 136 Draw3R(a_,thetala, theta2a,theta3a) ; figure; plot(ta, wOla, ta, wOla+ewOla, ta, wl2a, ta, wl2a+ewl2a, . . . ta, w23a, ta, w23a+ew23a) , ... title ( 'Actual and Desired Angular Velocities'); legend ( 'omegaOl' , 'omega01_d' , 'omegal2' , 'omegal2_d' , . . . 'omega23 ' , 'omega23_d' ) ; xlabel ( 'time' ) ; figure; plot(ta, ewOla, ta, ewl2a, ta, ew23a) , ... title { 'Errors in Joint Angular Velocities'); legend (' error wOl', 'error wl2 ' , 'error w23 ' ) ; xlabel ( ' time' ) ; figure; plot(ta, alphaOla, ta, alphaOla+ealOla, ta, alphal2a, ... ta, alphal2a+eall2a, ta, alpha23a, ta, alpha23a+eal23a) , . . . title ( 'Actual and Desired Angular Accelerations'); legend CalphaOl' , 'alpha01_d' , 'alphal2' , 'alphal2_d' , . . . 'alpha23' , 'alpha23_d') ; xlabel ( 'time' ) ; figure; plot(ta, ealOla, ta, eall2a, ta, eal23a) , ... title ( 'Errors in Joint Angular Accelerations'); legend ( 'error wOl' , ' error wl2 ' , 'error w23'); xlabel ( 'time' ) ; % figure; %plot{ta, wOla, ta, ta, wl2a, ta, w23a) , title ( 'Joint Angular Velocities ' ) ; %f igure; %plot(ta, alphaOla, ta, alphal2a, ta, alpha23a) , titleCJoint Angular Accelerations' ) ; end 137 % Movplatlcn % Program for path tracking of a parallel manipulator. % Program uses the function reverse66.m to compute % the Leg lengths, velocities, and accelerations t=0; % The initial pose of the platform off the path cO_=[0 ; ; .5001] ; cdO_=[0 ; ; ] ; cdd0_=[0 ; ; ] ; RMatO=[ cos(5*t^2+0.005) -sin (5*t*2+0 . 005) 0; sin (5*t*2+0 . 005) cos(5*t*2+0.005) 0; 1]; wm0=[0 -10*t+0.05 0; 10*t + 0.05 ; 0] ; amO=[0 -10.0 0; 10 . 0; 0] ; % The initial and final time for the tracking are tO = 0.0; .. tf=4.0; delta=0.09; % TIME STEP %Initialization of the arrays ta=[]; Legsa= [] ; VLegsa= [] ALegsa= [] Acoefa= [] Posea= [] ; ■- '\i t'.- • <<-. %Performing the Inverse Position and Velocity analysis for the initial position and Velocity. [LegsO, VLegsO, ALegsO] = reverse66 (cO_,RMatO, cd0_, wmO, cddO_, amO) ; %The loop starts here while t<tf t=t+delta; % The path to be followed by the moving platform C_=[0 ; 0.1 ; 0.5 ] ; Cd_= [ ; ; ] ; cdd_=[0 ; ; ] ; RMat= [cos(5*t*2) -sin(5*t*2) 0; sin(5*t*2) cos(5*t*2) 0; 1] ; wm=[0 -10*t 0; 10*t 0; 0]; am=[0 -10 0; 10 0; 0] ; [Legs, VLegs, ALegs] = reverse66 (c_,RMat, cd_, wm, cdd_, am); 138 for i=l:6 •; a3(i)=0.5*( 20*(Legs(i) -LegsO(i) ) - delta* (8*VLegs ( i)+12*VLegs0(i) ) + delta*2* (ALegs(i)+7*ALegsO(i) ) )/delta"3; a4(i)=- ( 15* (Legs(i) -LegsO (i) ) - delta* (7*VLegs (i) +8*VLegs0(i) ) ... + delta"2*(ALegs(i)+6*ALegsO(i) ) ) /delta 4; a5(i)=0.5*( 12* (Legs (i) -LegsO (i) ) - 6* delta* (VLegs ( i)+VLegs0(i) ) ... + delta*2*(ALegs(i)+5*ALegsO(i) ) )/delta^5; end %Updating the arrays ta=[ta ; [t]] ; Legsa= [Legsa ; [Legs] ] ; VLegsa= [VLegsa ; [VLegs] ] ; ALegsa= [ALegsa; [ALegs] ] ; Acoef a= [Acoef a ;[a3(l) a4(l) a5(l) a3(2) a4(2) a5(2) a3(3) a4(3) a5(3) a3(4) a4(4) a5(4) a3(5) a4(5) a5(5) a3(6) a4(6) as (6)] ]; Posea=[Posea ;[RMat(l,:) RMat(2,:) RMatC i, :) c_']] ; %Update the information cO_=c_; ^ cdO =cd ; ^.v .,i-. f-y- . , , „ cddO_=cdd_; ■■ ^, V-' ' ' '-'^^ ^ \ntP^ ■. . '■' -If RMatO=RMat; wmO=wm; amO=am; end figure; axis equal; plot(ta, Legsa (:,1), '-', ta, Legsa ( : ,2), '--', ta Legsa(:,3), ':', ta, Legsa ( : ,4) , '-.',... ta, Legsa(:,5), '+', ta, Legsa(:,6), / / ... title ('Leg Displacements of the Platfc 3rm) ' ) ; legendCLegKl) ' , 'Leg2(q) ' , 'Leg3(m) ' , 'Leg4(r) ', ... 'Leg5(n) • , •Leg6(p) ' ) ; %axis([0.0, 1.0, 10, 30]); ylabel ( ' in. ' ) ; xlabel ( 'time' ) ; figure; axis equal; plot(ta, VLegsa (:,1), '-', ta, VLegsa (:, 2), '--', \ :a, VLegsa ( : ,3) , ■ : ' , ta, VLegsa (:, 4) , •-. ' , ... ta, VLegsa (:, 5), '+', ta, VLegsa (:, 6) , ■"'), ... title ('Leg Velocities of the Platform '); legendCLegKl) ' , 'Leg2(q) ' , 'Leg3(m) ' , 'Leg4(r) ', ... 'Leg5(n) ' , 'Leg6(p) ') ; %axis([0.0, 1.0, -15, 25]); ylabel ( ' in. /sec' ) ; xlabel ( 'time' ) ; figure; ' 139 axis equal; plot(ta, ALegsa(:,l), '-', ta, ALegsa(:,2), '--', ta, ALegsa(:,3), ':', ta, ALegsa(:,4), '-.',... ta, ALegsa(:,5), '+', ta, ALegsa(:,6), '*'), ... titleCLeg Accelerations of the Platform'); legend ('Legl(l) ' , 'Leg2(q) ' , 'LegSdn) ' , 'Leg4(r) ' , . . . 'Leg5(n) ' , 'Leg6(p) ' ) ; %axis( [0.0, 1.0, -30, 50] ) ; ylabel ( ' in. /sec*2 ' ) ; xlabel ( ' time ' ) ; *»»»* 140 % reverse66 .n % previous: reverse_position66 .m (pos & vel) % % This function computes the leg (connector lengths) of % the Special 6-6 parallel platform % the platform is parametrized using % SIDE: determines the side of the platform (equi. triangle) % bscale: scales the distance of the pivot point as a fraction of SIDE % tsidescale: scales the side of the top platform % tsacle: scales the distance of the pivot point on the top platform % as a fraction of {tsidesacle*SIDE) % hscale: determines the height of the top platform w.r.t base % ( again, as a fraction of SIDE) % % The INPUTS are the center of the top platform (c_) and the rotation % Matrix of the top platfom RMat . Also cd_ is the velocity of the center of top plat % and wm is the angular velocity of top plat (it is a skew symmetric matrix) % AND the size of the platform in terms of the params . shown above % as a 5x1 vector. % The OUTPUT vectors are the 6 leg lengths and the velocities of the 6 legs function [Legs, VLegs, ALegs] = reverse_position_66 (c_, RMat, cd_, wm, cdd_, am, size) if nargin < 6 error ('6-6 inv. pos. analysis requires 6 or 7 inputs'), end if nargin > 7 error ('6-6 inv. pos. analysis requires 6 or 7 inputs'), end if nargin == 6 % Platform size size (1 1) = 3 0.0; %SIDE size (2 1) = 0.8 %bscale size(3 1) = 1.0 %tsidescale size (4 1) = 0.8 %tscale size (5 1) s 1.0 %hscale end , SIDE=size(l,l) ; bscale=size (2,1) ; tsidescale=size (3, 1) ; tscale=size (4,1) ; hscale=size (5, 1) ; % forming the transform matrix from c_ and RMat % and from cd_ and wm for i = l:3 - ■, \. ■ for j=l:3 TMat ( i , j ) =RMat ( i , j ) ; | - TVMat ( i , j ) =wm ( i , j ) ; TVMataux ( i , j ) =wm ( i , j ) ; ' „ , . ■ TAMat (i, j)=am(i, j) ; ,.,. ' ; ' ■ end TMat(i,4)=c (i,l) ; 141 TVMat (i,4)=cd_(i,l) ; -■■■.;, TVMataux(i,4) =0; TAMat (i,4)=cdd_(i,l) ; end; TMat(4, :) = [0,0,0,1] ; ' TVMat (4, :) = [0,0,0,1] , • TVMataux(4, :)=[0,0,0,0] ; TAMat(4, :)=[0,0,0,1] ; % Defining the joint locations on the base platform (fixed) pl= [SIDE/2; -SIDE/(2*sqrt(3) ) ; 0;1]; p2=[(l-bscale)*SIDE/2; (bscale- (1/3) ) *SIDE*sqrt (3) /2;0;1] ; p3=[0;SIDE/sqrt(3) ;0;1] ; p4=[-bscale*SIDE/2; - (bscale- (2/3) ) *SIDE*sqrt (3) /2 ; 0; 1]; p5= [-SIDE/2; -SIDE/ (2*sqrt(3) ) ;0;1] ; p6=[SIDE*(bscale-0.5) ; -SIDE*sqrt (3) /6 ; ; 1]; pMat= [pi p2 p3 p4 p5 p6] ; %pMat % Defining the joint locations on the top platform w.r.t to the % center of the top platform tSIDE=tsidescale*SIDE; ptl= [tscale*tSIDE/2 ; (tscale- (2/3 ) ) *tSIDE*sqrt (3 ) /2 ; ; 1] ; pt2=[tSIDE/2; tSIDE/ (2*sqrt (3 ) ) ;0;1] ; pt3=[-tSIDE*( tscale- 0.5) ; tSIDE/ (2*sqrt (3) ) ;0;1] ; pt4=[-tSIDE/2; tSIDE*sqrt (3 ) /6 ; ; 1] ; pt5=[- (l-tscale)*tSIDE/2; - (tscale- (1/3) ) *tSIDE*sqrt (3) /2;0;1] ; pt6=[0; -tSIDE/sqrt(3) ; 0; 1]; ptMat=[ptl pt2 pt3 pt4 pt5 pt6] ; % Finding the joint locations on the top platform w.r.t the % base platform (fixed) ptMat f ix=TMat *ptMat ; %ptMatfix VMat=ptMatf ix-pMat ; VelMat=TVMat*ptMatfix; ' VelMataux=VelMat ( 1 : 3 , 1 : 6 ) ; AccMat=TAMat*ptMatf ix+TVMataux* (TVMataux*ptMatf ix) ; for i=l:6 Legs (i) =sqrt (VMat ( : , i) ' *VMat ( : , i) ) ; end Legmat= [Legs; Legs; Legs; Legs]; ^, uVMat=VMat . /Legmat ; for i=l:6 VLegs(i)=uVMat( : ,i) '*VelMat (: ,i) ; "■ ALegs ( i ) =uVMat ( : , i ) • *AccMat ( : , i) + (norm(VelMataux( : ,i) )*2/Legs(i) ) VLegs(i)*2/Legs(i) ; end 142 % |Movplat2b.iD % Program for path tracking of a parallel manipulator. % Program determines the forces in each leg also % REQUIRES THE FUNCTION "reverse66f .m" % Implementing a constant acceleration motion (translation) with % the Weight of the top platform ignored W = 0. t=o; - ■ .: % The initial pose of the platform off the path C0_=[0.2 ; 0.4 ; 0.62 + 0.06*t*2]; Cd0_=[0 ; ; 0.06*2*t+.01] ; cdd0_=[0 ; ; 0.06*2+. 001 ] ; RMatO=[ 1 0; 1 0; 1] ; wm0= [0 0; 0; 0] ; amO=[0 0; 0; 0] ; % The initial and final time for the tracking are tO=0.0; tf=0.50; delta=0.02; % TIME STEP - %Initialization of the arrays ta=[]; Legsa= [] ; VLegsa= [] ALegsa= [] Acoefa= [] Posea=[]; '' ■ ,.^ -^ \ i ■■■' l'' ':*'V4 FLegsa= [] ; " * ' ■'' '■'' '' ' %Performing the Inverse Position and Velocity analysis for the initial position and Velocity. [LegsO, VLegsO, ALegsO, FLegsO] = reverse66f (cO_,RMatO, cdO_, wmO, cddO_, amO) ; %The loop starts here while t<tf t=t+delta; % The path to be followed by the moving platform C_=[0.2 ; 0.4 ; 0.6 + 0.06*t"2]; ',- Cd_=[ ; ; 0.06*2*t] ; cdd_=[0 ; ; 0.06*2] ; • , RMat=[l 0; 1 0; 1] ; wm= [0 0; 0; 0]; am= [0 0; 0; 0] ; [Legs, VLegs, ALegs, FLegs] = reverse66f (c_,RMat, cd_, wm, cdd_, am) ; for i=l:6 a3(i)=0.5*( 20* (Legs (i) -LegsO (i) ) - delta* (8*VLegs (i) +12*VLegsO (i) ) 143 + delta"2* (ALegs (i)+7 *ALegsO(i) ) )/delta*3; a4(i)=- ( 15*(Legs{i) -LegsO(i)) - delta* (7*VLegs (i) +8*VLegsO(i) ) ... + delta*2* (ALegs (i)+6 *ALegsO(i) ) ) /delta 4; a5(i)=0.5*( 12*(Legs( L)-LegsO(i)) - 6* delta* (VLegs ( i)+VLegsO (i) ) ... + delta*2* (ALegs (i)+5 * ALegs (i) ) )/delta*5; end %Updating the arrays - " !.-''*- ■■ ta= [ta ; [t] ] ; Legsa= [Legsa ; [Legs] ] ; VLegsa= [VLegsa ; [VLegs] ] / ■ ' ALegsa= [ ALegsa ; [ALegs ] ] t Acoef a= [Acoef a ;[a3(l) a4(l) a5(l) a3(2) a4(2) a5{2) a3(3) a4(3) a5(3) a3(4) a4(4) a5(4) a3(5) a4(5) a5(5) a3(6) a4(6) a5(6)] ] ; Posea= [Posea ; [RMat (1, : ) RMat (2, :) RMatC , :) c_']] ; FLegsa= [FLegsa; [FLegs ' ] ] ; %Update the information cO_=C_; .^ ,. . ' -i -vr uv^v"f^'f^,^,rl cdO_=cd_; ' * •' ' ■'-' cddO_=cdd_; ■at t % ;■ " . ^ : ;■-*" '■ " RMatO=RMat; .- ^ -J^^ [^ ; , ' »i wmO=wm; amO=am; ^\ _ *. ; •' ' ^-.•' ;V; -'r-^H f \ '■ - % ,. end % some plotting figure; axis equal; plot(ta, Legsa (:,1), 1 _ 1 1 ta, Legsa (: ,2), ■--', ta , Legsa ( : ,3) , ' : ' , ta, Legsa ( : ,4) , ' - . ' , . . . ta, Legsa(:,5), 'V , ta. Legsa ( : ,6) , 1 •*■ 1 \ f 1 . . • title ('Leg Displacements of the Platform' ) ; legend CLegKl) ' , 'Leg2(q) ' , 'Leg3(m) ' , 'Leg4(r) • , . . . 'Leg5(n) ' , 'Leg6(p) '); %axis([0.0, 1.0, 10, 30]) / ylabel('m') ; xlabel ( ' time ' ) ; figure; "-- .: : axis equal; plot(ta, VLegsa (:,1), t _ ' , ta, VLegsa (: ,2) , '--' , ta, VLegsa (: ,3) , ' : • , ta, VLegsa (: ,4) , ' - I . . ta, VLegsa (:, 5), ' V , ta. VLegsa ( : ,6) 1 * 1 N f It''' title ('Leg Velocities J of the Platform '); legend ( 'Legl(l) ' , 'Leg2 (q) ', 'Leg3(m) ', •Leg4(r) ', ... 'Leg5(n) ' , 'Leg6(p) •); %axis([0.0, 1.0, -15, 25]); xlabel ( 'time' ) ; ylabel ( 'm/sec' ) ; figure; axis equal; 144 plot(ta, ALegsa(:,l), '-', ta, ALegsa(:,2), '--', ta, ALegsa(:,3), :', ta, ALegsa(:,4), '-.',... ta, ALegsa(:,5), ' V , ta, ALegsa{:,6), '^'), ... titleCLeg Accelerations of the Platform'); legendCLegKD • , 'Leg2(q) ' , 'Leg3(m) ' , 'Leg4(r) • , . . . 'Leg5(n) • , •Leg6(p) ') ; %axis([0.0, 1.0, -30, 50]); xlabel ( 'time' ) ; ylabel('m/sec*2') ; f igure ; axis equal; plot(ta, FLegsa(:,l), '-', ta, FLegsa(:,2), '--', ta, FLegsa(:,3), ' : ' , ta, FLegsa( :, 4), '-.',... ta, FLegsa(:,5), ' V , ta, FLegsa(:,6), '^'), ... titleCLeg Forces of the Platform'); legendCLegKD ' , 'Leg2(q) ' , 'Leg3(m) ' , 'Leg4(r) ', .. . 'Leg5(n) ' , 'Leg6(p) ' ) ; %axis([0.0, 1.0, -30, 50]); xlabel ( • time ' ) ; . ; _ ylabel('N'); i r> :'\.ii ' *' ' ^ ^ 145 % |Movplat2d.in| % Program for path tracking of a parallel manipulator. % Program determines the forces in each leg also % REQUIRES THE FUNCTION "reverse66f .m" % Implementing a rotation around c (about z) , no translation, % and constant angular acceleration % the Weight of the top platform ignored W = 0. % IMPORTANT ***** change Weight in "reverse66f .m" t = 0; % The initial pose of the platform off the path cO_=[0 ; ; .5001] ; cdO_=[0 ; ; ] ; cddO_= [0 ; ; ] ; RMat0=[ cos(5*t"2+0.005) -sin (5*t*2+0. 005) 0; Bin(5*t 2+0.005) cos(5*t*2+0.005) 0; 1]; wmO=[0 -10*t + 0.05 0; 10*t+0.05 ; 0] ; amO=[0 -10.0 0; 10.0 0; 0] ; % The initial and final time for the tracking are tO=0.0; tf=0.50; delta=0.02; % TIME STEP %Initialization of the arrays ta=[]; Legsa= [] ; VLegsa= [] ALegsa= [] Acoefa= [] Posea= [] ; „■ FLegsa= [] ; %Performing the Inverse Position and Velocity analysis for the initial position and Velocity. [LegsO, VLegsO, ALegsO, FLegs0]= reverse66f (c0_,RMat0, cdO_, wmO, cddO_, amO) ; %The loop starts here while t<tf t=t+delta; % The path to be followed by the moving platform c_=[0 ; 0.1 ; 0.5 ] ; cd_= [ ; ; ] ; cdd_=[0 ; ; ] ; RMat=[cos(5*t"2) -sin(5*t*2) 0; sin(5*t*2) cos(5*t"2) 0; 1] ; wm=[0 -10*t 0; 10*t 0; 0]; am=[0 -10 0; 10 0; 0] ; [Legs, VLegs, ALegs, FLegs] = reverse66f (c_,RMat, cd_, wm, cdd_, am); 146 for i=l:6 a3{i)=0.5*( 20* {Legs(i) -LegsO (i) ) - delta* (8 * VLegs ( i)+12 *VLegsO(i) ) + delta*2* (ALegs(i)+7*ALegsO(i) ) ) /delt a*3; a4(i)=- ( 15* (Legs (i) -LegsO (i) ) - delta* (7*VLegs (i) +8*VLegs0(i) ) ... + delta*2* (ALegs(i)+6*ALegsO (i) ) )/delt a"4; a5(i)=0.5*( 12* (Legs (i) -LegsO (i) ) - 6" delta* (VLegs ( i)+VLegs0(i) ) ... + delta*2* (ALegs(i)+5*ALegsO(i) ) )/delt a*5; end %Updating the arrays ta=[ta ; [t] ] ; Legsa= [Legsa ; [Legs] ] ; VLegsa= [VLegsa ; [VLegs] ] ; ALegsa= [ALegsa; [ALegs] ] ; Acoef a= [Acoef a ; [a3 (1) a4(l) a5(l) a3(2) a4(2) a5(2) a i3(3) a4(3) a5(3) a3(4) a4(4) a5(4) a3 (5) a4(5 a5(5) a3(6) a4(6) a5(6)] ]; Posea=[Posea ;[RMat(l,:) RMat(2,:) RMat(3,:) c_ •]]; FLegsa= [FLegsa; [FLegs']]; %Update the information cO_=c_; ^ cdO_=cd_; cddO_=cdd_; RMatO=RMat; wmO=wm; amO=am; end % some plotting figure; axis equal; plot(ta, Legsa (:,1), '-', ta, Legsa ( : ,2), ■- ■-', ta , Legsa ( : ,3) , ' : ' , ta, Legsa(:,4), '-.', ... ta, Legsa{:,5), 'V , ta, Legsa(:,6), '"'), title('Leg Displacements of the Platform'); legendCLegld) ', 'Leg2(q) ', ■Leg3(m) ', •Leg4(r) ■ , . . . 'Leg5(n) ' , 'Leg6(p) ') ; %axis([0.0, 1.0, 10, 30]); ylabel ( 'm' ) ; xlabel('time') ; figure; '" • - ^ axis equal; plot(ta, VLegsa (:,1), '-', ta, VLegsa (:, 2), '--', ta, VLegsa (:, 3), ' : ' , ta, VLegsa ( : ,4) , '-.',... ta, VLegsa (:, 5), 'v', ta, VLegsa (:, 6), '^'), ... title ('Leg Velocities of the Platform'); legendCLegld) ' , •Leg2(q) ' , 'Leg3 (m) ' , •Leg4(r) ' , . . . 'Leg5(n) ' , 'Leg6(p) ') ; %axis([0.0, 1.0, -15, 25]); xlabel ( 'time' ) ; ylabel ( 'm/seC ) ; 147 figure; axis equal ; plot{ta, ALegsa(:,l), '-', ta, ALegsa(:,2), '-- ' : ' , ta, ALegsa(: ,4) , '-.',... ta, ALegsa(:,5), ' V , ta, ALegsa(:,6), '*'), •• titleCLeg Accelerations of the Platform'); legendCLegKD ' , 'Leg2(q) ' , 'Leg3 (m) ' , 'Leg4(r) ' , 'Leg5(n) • , 'LegeCp) ') ; %axis( [0.0, 1.0, -30, 50] ) ; xlabel ( 'time' ) ; ylabel('m/sec*2') ; ta, ALegsa( : ,3) , figure; ' axis equal; plot(ta, FLegsa(:,l), '-', ta, FLegsa(:,2), ' ' : ' , ta, FLegsa(: ,4),'-.',... ta, FLegsa(:,5), ' V , ta, FLegsa(:,6), '*'), title ('Leg Forces of the Platform'); legendCLegKD ' , 'Leg2(q) ' , 'Leg3(m) ' , 'Leg4(r) 'Leg5(n) ' , 'Leg6(p) ') ; %axis([0.0, 1.0, -30, 50]); xlabel ( 'time' ) ; ylabel('N'); ta, FLegsa( : , 3) , r*-^ V- 148 reverse66f .n % previous: reverse66.m (pos, vel and accel) % % This function computes the leg (connector lengths) of % the Special 6-6 parallel platform ALSO computes the % leg velocities, leg accelerations, and LEG FORCES % the platform is parametrized using % SIDE: determines the side of the platform (equi. triangle) % bscale: scales the distance of the pivot point as a fraction of SIDE % tsidescale: scales the side of the top platform % tsacle: scales the distance of the pivot point on the top platform % as a fraction of (tsidesacle*SIDE) % hscale: determines the height of the top platform w.r.t base % ( again, as a fraction of SIDE) % % The INPUTS are the center of the top platform (c_) and the rotation % Matrix of the top platfom RMat. Also cd_ is the velocity of the center of top plat % and wm is the angular velocity of top plat (it is a skew symmetric matrix) % AND the size of the platform in terms of the params. shovm above % as a 5x1 vector. % The OUTPUT vectors are the 6 leg lengths and the velocities of the 6 legs • % The acceleration and forces in each of the legs function [Legs, VLegs, ALegs, FLegs] = reverse_position_66 (c_, RMat, cd_, wm, cdd_, am, size) if nargin < 6 error ('6-6 inv. pos. analysis requires 6 or 7 inputs'), end if nargin > 7 error ('6-6 inv. pos. analysis requires 6 or 7 inputs'), end I2M=0.0254; % in to meter if nargin ==6 . ' % Platform size size (1,1) = 30.0*I2M; %SIDE size(2,l) = 0.8 size(3,l) = 1.0 size(4,l) = 0.8 size(5,l) = 1.0 end %bscale %tsidescale %tscale %hscale % Defining the Inertia Matrix (Mass in kgm, and Inertia in Kgm-m*2) % For explanation see Rico notes I=[0 79.5928 0; 79.5928 ; 79.5928; 3.39146 0;... 3.39167 0; 00 6.590532 000]; % Computing the Velocity and Reduced Acceleration States V=[wm(3,2) ; wm(l,3); wm(2,l); cd_(l,l); cd_(2,l); cd_(3,l)]; % Dual part of the Reduced accel vector aO-w X v DAR=cdd_-wm*cd_; AR=[am(3,2) ; am(l,3); am(2,l); DAR(1,1); DAR(2,1); DAR(3,1)]; 149 ■ ■"' ■ % Computing the Inertia Forces (weight for scr's platform) Weight=[0; 0; 0; ; 0; 0]; % W(-k) = • -795.928 % Wrench on the top platform w.r.t center of moving platform (moving ccs) Wc=I*AR+Lie(V,I*V) -Weight ; %This is an skew matrix for computing 1 the cross product of rc/C. Mc=[0 -c_(3,l) c_(2,l); c_(3,l) -c_(l,l); -c_(2,l) c_ (1,1) 0]; % Calculating MC = Mc + r X F Temp=Wc(4:6,l)+Mc*Wc(l:3,l) ; % Wrench on the top platform w.r.t the fixed coordinate syst 5m % by augmenting the F from Wc and MC f rom Temp (moment component) W= [Wc (1 : 3 , 1 ) ; Temp (1:3,1)]; SIDE=size(l,l) ; bscale=size (2, 1) ; tsidescale=size(3,l) ; tscale=size(4, 1) ; hscale=size(5, 1) ; % forming the transform matrix from c_ and RMat % and from cd_ and wm for i=l:3 for j =1:3 r ; ^ ■^;'■J; p -ht ■ TMat (i, j)=RMat (i, j) ; TVMat ( i , j ) =wm ( i , j ) ; TVMataux ( i , j ) =wm ( i , j ) ; TAMat(i, j)=am(i, j) ; end TMat(i,4)=c_(i,l) ; TVMat (i,4)=cd_(i,l) ; TVMataux (i, 4 )=0; - TAMat (i,4)=cdd_(i,l) ; end; TMat(4, :)=[0, 0,0,1] ; TVMat (4, :) = [0,0, 0,1] ; TVMataux (4, :)= [0,0,0,0] ; TAMat (4, : )=[0, 0,0,1] ; % Defining the joint locations on the base platform (fixed) pl= [SIDE/2; -SIDE/(2*sqrt(3) ) ; 0;1]; p2=[(l-bscale)*SIDE/2; (bscale- (1/3) )* SIDE*sqrt (3) /2;0 1]; p3=[0;SIDE/sqrt(3) ;0;1] ; p4=[-bscale*SIDE/2; - (bscale- (2/3) ) *SIDE*sqrt (3) /2; 0; 1]; p5=[ -SIDE/2; -SIDE/ (2*sqrt (3 ) ) ;0;1] ; p6=[SIDE*(bscale-0.5) ; -SIDE*sqrt (3) /6 ; ; 1] ; pMat= [pi p2 p3 p4 p5 p6] ; %pMat % Defining the joint locations on the top platform w.r .t to the % center of the top platform tSIDE=tsidescale*SIDE; ptl=[tscale*tSIDE/2; (tscale- (2/3) ) *tSIDE*sqrt (3) /2; 0;! L]; 150 pt2=[tSIDE/2; tSIDE/ (2*sqrt (3 ) ) ; ; 1] ; ' pt3=[-tSIDE*(tscale-0.5) ; tSIDE/ (2*sqrt (3) ) ; ; 1] ; pt4=[-tSIDE/2; tSIDE*sqrt (3) /6 ; ; 1] ; pt5=[-(l-tscale)*tSIDE/2; - (tscale- (1/3) ) *tSIDE*sqrt (3) /2;0;1] ; pt6=[0; -tSIDE/sqrt(3) ; 0; 1]; ptMat= [ptl pt2 pt3 pt4 pt5 pt6] ; % Finding the joint locations on the top platform w.r.t to the center of % the top (moving) platform, expressed in the fixed coordinate system. ptMatf ix=TMat*ptMat ; %ptMatfix VMat=ptMatfix-pMat; VelMat=TVMat*ptMatfix; VelMataux=VelMat (1:3,1:6) ; AccMat=TAMat*ptMatf ix+TVMataux* (TVMataux*ptMatf ix) ; for i=l:6 Legs(i)=sqrt (VMat(: ,i) '*VMat ( : ,i) ) ; ■ > end Legmat= [Legs; Legs; Legs; Legs]; %VMat J.: %Legmat %VelMat uVMat=VMat . /Legmat ; %uVMat for i=l:6 JL(:,i)=Cross((c_+ ptMatf ix(l : 3, i) )' ,uVMat (1 :3, i) ')' ; % Dual part of the screw VLegs ( i ) =uVMat ( : , i) ' *VelMat ( : , i) ; ALegs ( i ) =uVMat ( : ,i) ' *AccMat ( : , i) + (norm(VelMataux( : ,i) ) ^2/Legs(i) ) -. . VLegs (i)*2/Legs(i) ; end J= [uVMat (1:3, :) ; JL] ; % The Jacobian Matrix %Solve the force analysis FLegs=J\W; % The force is in Newtons 151 % |inaln54cPIDl.ir July 2000 An example for a 3R planar serial chain to study the proposed path tracking methodology using initial position and final position and velocity data Program requires the following functions: / Screw / Draw3R Indues a PID Controller for a 3R Manipulator Previous: main54c.m Last Update: June 15, 2001 D2R = pi/180.; % Degrees to Radians conversion factor %a = [ 1 ; 1 ; 1] ; % Constant data, link lengths a_= [ .3 ; .3 ; .15] ; r = 0.1; % r is a constant required by the end-effector path. % The initial and final time, and the time step are given below to = 0.0; tf=2.0; delta = 0.01 ; . % MOTOR Data K = 1; % Gain for the system (motor) tau = [ 0.005753 ; 0.00063 ; 0.0017 ]; % Motor time constants % Kp = [ .75 ; 0.5 ; .426 ]; % Proportional gains (from sysmodelx.m) % Kd = [ .0002 ; 0.000166 ; .0001076 ] ; % Derivative gains % Ki = [ 285.8 ; 199.84 ; 200.33 ] ; % Integral gains Kp = [ 1.077 ; 0.6156 ; 1.0626 ]; % Proportional gains (from sysmodelx.m) Kd = [ .000404 ; 0.000166 ; .0001076 ] ; % Derivative gains Ki =[ 71.8 ; 56.84 ; 262.33 ] ; % Integral gains D = 0.03; % Delay in seconds % variables deltac = delta/10; % Determines the speed at which the PID control loop will run % Constant for the DISCRETE CONTROLLER kO = Kp + Ki*deltac + Kd/deltac; kl = -Kp - (2*Kd/deltac) ; k2 = Kd/deltac; for i=l:l:3 el(i)=Oi e2(i)^ e3(i)^ end for i=l:l: (2+round(D/delta) ) % xl(i)=0; x2(i)^ x3(i)^ end for end i=l:l:2 wal(i)=0 wa2 (i) =0 wa3 (i) =0 152 %Current Position Data: initial position before starting re = [0.5995 ; 0.01 ; 0] ; thetac = ; [setlc, set2c, flagc] = reverse_position_3R(a_, re, thetac); if flagc == 1 error ('The initial position is outside the manipulator workspace, there is no point in completing the problem') else % Initializing all the vector arrays ta = [] ; rOxa = [] ; rOya = [] ; thetaa= [] ; thetala=[]; theta2a= [] ; theta3a= [] ; w01a=[]; wl2a=[]; w23a= [] ,■ alpha01a= [] ; alphal2a= [] ; alpha23a= [] ; ew01a=[]; ewl2a= [] ; ew23a=[]; eal01a=[]; eall2a= [] ; eal23a=[]; w01f=[]; wl2f=[]; w23f=[]; Wal=[]; Wa2=[]; Wa3= [] ; errorwl=[]; errorw2= [] ; errorw3= [] ; % The Loop starts over here for the duration tf while tO<tf %The desired motion and velocity of the end-effector is given below rd = [0.5+r*cos( (tO+delta) ) ; r*sin( (tO+delta) ) ; 0] ; thetad =0.0; Vd= [0;0;0;-r*sin(tO+delta) ;r*cos (tO+delta) ; 0] ; As=[0;0;0;-r*cos(tO+delta) ;-r*sin(tO+delta) ; 0]; wO=Vd(l:3,l) ; . ; vO=Vd(4:6,l) ; ' " •""- ' CV=Cross(wO' ,vO' ) ' ; Ard=[As(l,l); As(2,l); As (3 , 1) ; As (4 , 1) -CV(1 , 1) ;As (5 , 1) - CV(2,1) ;AS(6,1)-CV{3,1)] ; % The following statement compute the reverse analysis of the manipulator % for the required position at time tO+delta. [setl, set2, flag] = reverse_position_3R(a_, rd, thetad); if flag == 1 error ( ' The required position is outside the manipulator workspace' ) . . else [Jd, waste] = Screw (a_, setl) ; % wd are the join angular velocities computed from the desired % velocity state wd=Jd\Vd; ' , . Jwd=Jd*diag(wd) ; alphad=Jd\ (Ard-Paccst ( Jwd, 3 , 1) ) ; 153 % w are the join angular velocities and acce ilerations computed from our % tracking a w=2* (setl-se %alpha=2* (s€ ilgorithm itlc) /delta-wd; >tl-setlc)/delta"2; alpha= (2/delta) * (wd- ( (setl-setlc) /d( 2lta)) ; % PID CONTROLLER WITH THE PLANT tcl=0; while ( tcl < delta ) el(l) = w(l,l) - wal(l) ; e2{l) = w(2,l) - wa2(l) ; e3(l) = w(3,l) - wa3(l) ; xl(l) = xl(2) + kO(l,l)*el(l) + kl(l,l) *el(2) + k2(l l)*el(3) ; x2(l) = x2(2) + k0(2,l)*e2(l) + kl(2,l) *e2(2) + k2(2 l)*e2(3) ; x3(l) = x3 (2) + k0(3,l)*e3 (1) + kl(3,l) *e3(2) + k2(3 l)*e3(3) ; wal(l) = exp(-deltac/tau(l,l) )* wal (2) + K*(l - exp(- cielt< ac/tau(l,l) ) ) *xl(l+round(D/delta) ) ; wa2 (1) = exp(-deltac/tau(2,l) )* wa2(2) + K* (1 - exp ( - delt ac/tau(2,l) ) ) *x2 {l+round(D/delta) ) ; wa3(l) = exp(-deltac/tau(3,l) )* wa3(2) + K* (1 - exp(- delt ac/tau{3,l))) wal(2) wa2(2) wa3(2) *x3 (l+round(D/delta) ) ; = wal(l) ; = wa2(l) ; = wa3 (1) ; el(3) = el(2); el(2) = el(l); 62(3) = e2(2); e2(2) = e2(l) ; e3{3) = e3(2); e3(2) = e3(l); for j = length (xl) :-l:2 xl( j) = xl(j-l); x2{ j) = x2(j-l); x3( j) = x3(j-l); end tcl = tcl + deltac; end % Array of actual velocities (from the control loop) Wal = [ Wal [ wal(l) ]] ; Wa2 = [ Wa2 [ wa2(l) ]] ; Wa3 = [ Wa3 [ wa3(l) ]] ; % Array of velocity errors (between actual ang vel . ' wa ' and command velocity 'w') errorwl = | errorwl [ wal(l) - w(l, 1) ]]; errorw2 = | errorw2 [ wa2(l) - w(2, 1) ]]; errorw3 = 1 errorw3 [ wa3(l) - w(3, 1) ]] ; wf=w+alpha* delta; 154 % Calculating Errors in Position, Orientation, % Linear and Angular velocity ta = [ ta [ to + delta ] ] ; rOxa = [ rOxa [ rd(l,l) ] ] ; ' rOya = [ rOya [ rd(2,l) ] ] ; thetaa = [thetaa [thetad] ] ; thetala = [thetala [setl(l,l)]] theta2a = [theta2a [setl(2,l)]] theta3a = [theta3a [setl(3,l)]] w01a=[w01a [ w(l,l)] ] ; wl2a= [wl2a [ w(2,l) ] ] ; w23a=[w23a [ w(3,l)] ] ; w01f=[w01f [wf(l,l)]]; Wl2f=[wl2f [wf(2,l)]]; w23f=[w23f [wf(3,l)]]; ew01a=[ew01a [wf (1, 1) -wd(l, 1) ] ] ewl2a=[ewl2a [wf (2, 1) -wd(2 , 1) ] ] ew23a=[ew23a [wf (3, 1) -wd(3 , 1) ] ] % Updating for the next iteration ;. to = to + delta; tn ' --^i setlc=setl; end end figure; axis normal; plot(ta, wOla, '--', ta, Wal, '-', ta, wl2a, '-.', ta, Wa2, w23a, ' : ' , ta, Wa3, ' -' ) , ... title ( 'Actual and Command Angular Velocities'); legend ( ' omegaOl ' , ' omega01_c ' , ' omegal2 ' , ' omegal2_c ' , ' omega23 ' , ' omega23_c ' ) ; xlabel ( 'time' ) ; figure; plotita, errorwl, '-', ta, errorw2, '--', ta, errorw3, ':') title ( 'Errors in Joint Angular Velocities'); legend ( ' error wl ' , ' error w2 ' , ' error w3 ' ) ; xlabel { 'time' ) ; . ta. %f igure; %plot(ta, wOla, ta, ta, wl2a, ta, w23a) , title ( 'Joint Angular Velocities ' ) ; % figure; %plot(ta, alphaOla, ta, alphal2a, ta, alpha23a) , titleCJoint Angular Accelerations' ) ; - LIST OF REFERENCES AbbOO Abbasi, W. A., Ridgeway, S. C, Adsit, P. D., Crane, C. D., and Duffy, J., "Investigation of a Special 6-6 Parallel Platform for Contour Milling." Trans, of the ASME, Journal of Manufacturing Science and Engineering, Vol. 122, No. 1, February 2000, pp: 132-139. Abe92 Abe, S. and Tsuchiya, T., "Robot Manipulator Path Control Based on Variable Speed Trajectory Planning." Advanced Robotics, Vol. 6, No. 1, 1992, pp: 1- 13. Ami96 Amin, S. and Ahtiwash, 0. M., "Performance of Two Neuro Controllers for Robot Path Planning Control." Proc. 1996 IEEE 22"'' Intl. Conf on Industrial Electronics, Control, and Instrumentation, August 1996, Taipei, Taiwan, Vol. 3,pp: 1856-1861. BalOO Ball, R. S., .4 Treatise on the Theory of Screws, Cambridge University Press, 1900. Paperback edition, 1999. B0I88 Bollinger, John G. and Duffie, Neil A., Computer Control of Machines and Processes, Addison- Wesley Publishing Company, 1988. Cah96 Cahill, A. J., Kieffer, J. C, and James, M. R., "Robust Time-Optimal Trajectory Planning for Robot Manipulators." IEEE Robotics and Automation Conference, 1996. Cha30 Chasles, M., "Note sur les proprietes generales du systeme de deux corps semblables entr'eux et places d'une maniere quelconque dans I'espace; et sur le deplacement fmi ou infmiment petit d'un corps solide libre." Ferussac, Bulletin des Sciences Mathematiques, Vol. xiv., pp: 321-326, 1830. Cra99 Crane, C. and Duffy, J., "Geometry of Mechanisms and Robots II," Course notes for EML 6282, Graduate Course, Dept. of Mechanical Engineering, University of Florida, 1999. Du95 Du, S., Tang, J., and Yang, G., "A New Method of Discrete Trajectory Plarming for Robot Manipulators. " International Journal of Robotics and ^Mtowaf/on, Vol. 10, No. 4, 1995, pp: 123-129. Fla88 Flash, T. and Potts, R. B., "Discrete Trajectory Planning." International Journal of Robotic Research, Vol. 6, No. 1, 1988, pp: 48-57. 155 156 Gra93 Grasa, P., Volberg, 0., and Polyakhov, N., "Direct Adaptive Trajectory Control of Robot Manipulators via Fuzzy Logic Algorithm." Proc. 8"' Intl. Conf. on Applications of AI in Engineering, Toulouse, France, Vol. 2, 1993, pp: 561-569. Gri93 Griffis, M. and Duffy, J., "Method and Apparatus for Controlling Geometrically Simple Parallel Mechanisms with Distinctive Connections." US Patent No. 5,179,525, January 12, 1993. Hun78 Hunt, K. H., Kinematic Geometry of Mechanisms, Oxford University Press, 1978. Kie96 Kieffer, J. C, Cahill, A. J., and James, M. R., "Closed-Loop Trajectory Generation for Robust Time-Optimal Path Tracking." Proc. of the 1996 IEEE International Conf On Robotics & Automation." Minneapolis, MN, April 1996, pp: 1559-1565. Kie97 Kieffer, J., Cahill, A. J., and James, M. R., "Robust and Accurate Time- Optimal Path-Tracking Control for Robot Manipulators." IEEE Trans. Robotics and Automation, Vol. 13, No. 6, Dec. 1997, 880-890. Li98 Li, Q., Tso, S. K., and Zhang, W. J., "Control of Robot Manipulators Using a Dual-Model-Based Structure." Proc. of 1 998 ASME DETC, September 13-16, Atlanta, GA. Lin83 Lin, C. S., Chang, P. R., and Luh, J. Y. S., "Formulation and Optimization of Cubic Polynomial Joint Trajectories for Industrial Robots," IEEE Trans, on Automatic Control, Vol. AC-28, No. 12, 1983, pp:1006-1017. Luh77 Luh, J. Y. S. and Walker, M. W., "Minimum-time Along the Path for a Mechanical Arm." Proc. 16"' Conf Decision Contr., Dec. 1977, pp:755-759. LuhSl Luh, J. Y. S. and Lin, C. S., "Optimum Path Planning for Mechanical Manipulators." ASME Journal of Dynamical Systems, Measurement, and Control., Vol. 102, June 1981, pp: 142-151. Mar96 Martinez, J. M. R. and Duffy, J., "An Application of Screw Algebra to the Acceleration Analysis of Serial Chains," Mechanism and Machine Theory, Vol. 31, No. 4, pp: 445-457, 1996. Mas99 Mason, P. A. C, Walchko, K., and Crane, C. D., "A MIMO Fuzzy Controller, for Tracking: Robot Control." 1999 Florida Conf. on Recent Advances in Robotics, April 1999, Gainesville, FL. http://cimar.me.ufl.edu/FLA99/papers.html . 157 Moo79 Moore, R. E., Methods and Applications of Interval Analysis. Philadelphia, PA. SI AM Press, 1979. Naj96 Najson, F. and Kreindler, E., "Robot Robust Path Tracking." Dynamics and Control, Vol. 6, 1996, pp: 333-359. Oga97 Ogata, K., Modern Control Engineering, 3"* Edition, Prentice-Hall, 1997. Par97 Park, S. and Park, C. H., "Model-based Path Planning and Tracking Control Using Neural Networks for a Robot Manipulator." Conf. Proc. of the 1997 IEEE Intl. Conf on Neural Networks, Houston, TX, Vol. 3, June 1997, pp: 1761-1765. Pau81 Paul, R. P. C, Robot Manipulators: Mathematics, Programming, and Control. Cambridge, MA. M.I.T Press, 1981. Phi84 Phillips, J., Freedom in Machinery. Vol. I, Introducing Screw Theory, Cambridge University Press, 1984. Pia97 Piazzi, A., and Visioli, A., "An Interval Algorithm for Minimum-Jerk Trajectory Planning of Robot Manipulators. " Proc. Of the 36^'' Conf On Decision & Control, San Diego, CA, USA, December 1997. Poi06 Poinsot, L., "Sur la composition des moments et la composition des aires" (1804). Journal de I'Ecole Poly technique. Vol. vi. (13 ch.), pp: 182-205, 1806. Rag93 Raghavan, M., "The Stewart Platform of General Geometry has 40 Configurations." Trans, of the ASME, Journal of Mechanical Design, Vol. 115,June 1993, pp: 272-282. Rel Reliance Motor Control, Inc., DC Motors, Speed Controls, Servo Systems: The Electro-Craft Engineering Handbook, Reliance Motor Control Inc., Eden Prairie, Mirmesota. Ric99 Rico, J. M., Gallardo, J., and Duffy, J., "Screw Theory and Higher Order Kinematic Analysis of Open Serial and Closed Chains." Mechanism and Machine Theory, Vol. 34, pp: 559-586, 1999. Shi85 Shin, Kang G. and McKay, Neil D., "Minimum-Time Control of Robotic Manipulators with Geometric Path Constraints. " IEEE Transactions on Automatic Control, Vol. AC-30, No. 6, 1985, pp: 531-541. 158 Shi86a Shin, Kang G. and McKay, Neil D., "A Dynamic Programming Approach to Trajectory Planning of Robotic Manipulators." IEEE Transactions on Automatic Control, Vol. AC-31, No. 6, 1986, pp:49 1-500. Shi86b Shin, Kang G. and McKay, Neil D., "Selection of Near-Minimum Time Geometric Paths for Robot Manipulators. " IEEE Transactions on Automatic Conrro/, Vol. AC-31, No. 6, 1986, pp: 501-511. Shi87 Shin, K. J. and McKay, N. D., "Robust Trajectory Planning for Robotic Manipulators Under Payload Uncertainties." IEEE Trans. Automatic Control, Vol. AC-32, No. 12, 1987, pp: 1044-1054. Slo87 Slotine, J. J. E. and Li, W., "On the Adaptive Control of Robot Manipulators." InternationalJournal of Robotics Research, Vol. 6, 1987, pp: 49-59. Ste65 Stewart, D., "A Platform with Six Degrees of Freedom," Proc. Institution of Mechanical Engineers, Vol. 180, Pt. 1, No. 15, 1965, pp: 371-386. Tan88 Tan, H. H. and Potts, R. B., "Minimum Time Trajectory Planner for the Discrete Dynamic robot Model with Dynamic Constraints." IEEE Journal of Robotics and Automation, Vol. 4, No. 2, 1988, pp: 174-185. Tsu90 Tsuchiya, T. and Egami, T., "Robot Path Control with Variable Speed by Preview Control and Adaptive Control." IF AC 11'^ Triennial World Congress, Tallinn, Estonia, USSR, 1990, pp: 121-126. Wan93 Wang, S., Tsuchiya, T., and Hashimoto, Y., "Path Tracking Control of Robot Manipulators Utilizing Future Information of Desired Trajectory." Trans. Of Japan Society of Mechanical Engineers, Part C, Vol. 5, No. 564, Aug. 1993, pp: 2512-2518. Wan94 Wang, S. and Tsuchiya, T., "Fuzzy Trajectory Planning and its Application to Robot Manipulator Path Control." Advanced Robotics, Vol. 8, No. 1, 1994, pp: 73-93. Xia94 Xiangrong, X. and Xiangfeng, M., "A Method for Trajectory Planning of Robot Manipulators." Modeling, Measurement & Control B:, ASME Press, Vol.54,No. 3, 1994,pp:33-36. Zad69 Zadeh, L. A., "Fuzzy Sets." Information Control, Vol. 8, No. 3, 1965, pp: 338-353. Zel95 Zelek, J. S., "Dynamic Path Planning." Proc. of the IEEE International Conference on Systems, Man and Cybernetics, Vol. 2, 1995, Piscataway, NJ, pp: 1285-1290. BIOGRAPHICAL SKETCH Waheed A. Abbasi was bom on August 12, 1968 in the southern port-city of Karachi, Pakistan. To undertake his undergraduate studies, he moved to the Comhusker state, where he received his Bachelor of Science degree in Mechanical Engineering from the University of Nebraska-Lincoln (UNL) in 1991. His interest in Robotics and his desire to pursue graduate study brought him to Gator country, where he joined the Center for Intelligent Machines and Robotics (CIMAR), Department of Mechanical Engineering, University of Florida (UF). He received his Master of Science degree in Mechanical Engineering with a minor in Industrial Engineering in December 1994. Afterwards, he continued his studies and work at CIMAR, being involved in a variety of projects as well as conducting research. Upon completion of his doctoral degree, he intends to work in the industry. Waheed is married to Uzma and they have a son, Bassil. 159 I certify that I have read this study and that in my opinion it conforms to acceptable standards of scholarly presentation and is fliUy adequate, in scope and quality, as a dissertation for the degree of Doctor of Philosophy. Carl D. Crane III, Chairman Professor of Mechanical Engineering I certify that I have read this study and that in my opinion it conforms to acceptable standards of scholarly presentation and is fully adequate, in scope and quality, as a dissertation for the degree of Doctor of Philosophy. Joseph ouff Graduate Research Professor of Mechanical Engineering I certify that I have read this study and that in my opinion it conforms to acceptable standards of scholarly presentation and is fully adequate, in scope and quality, as a dissertation for the degree of Doctor of Philosophy. yM.^^> C. Ziegert essor of Mechanica^ngineering I certify that I have read this study and that in my opinion it conforms to acceptable standards of scholarly presentation and is fully adequate, in scope and quality, as a dissertation for the degree of Doctor of Philosophy. jhn K. Schueller Associate Professor of Mechanical Engineering I certify that I have read this study and that in my opinion it conforms to acceptable standards of scholarly presentation and is fully adequate, in scope and quality, as a dissertation for the degree of Doctor of Philosophy. ,1 -' Michael C. Nechyba Assistant Professor of Electrical and Computer Engineering This dissertation was submitted to the Graduate Faculty of the College of Engineering and to the Graduate School and was accepted as partial fulfillment of the requirements for the degree of Doctor of Philosophy. August 2001 . iP,..Xlww^ l< L..y^ Pramod P. Khargonekar Dean, College of Engineering Winfred M. Phillips Dean, Graduate School lO 200 1 DciTY OF FLORIDA liiiifi: